modularization/src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

#include "vehicle_upload.h"

#include <QFile>
#include <QString>
#include <QStringList>
#include <math.h>
#include <iomanip>
#include <thread>

#include "modulecomm.h"
#include "rawpic.pb.h"
#include "chassis.pb.h"
#include "platform_feedback.pb.h"
#include "gpsimu.pb.h"
#include "math/gnss_coordinate_convert.h" //for calculate totalPathLength

extern std::string gstrserverip;
extern std::string gstruploadPort;
extern std::string gstruploadInterval;
extern std::string gstrid;
extern std::string gstrplateNumber;
extern std::string gvehicleType;

extern double gspeedWantAvg; // m/s
extern double gwaitTime; // s

extern char stryamlpath[256];

extern uint8_t gShift_Status;//3 p 4 r 5 n 6 d
extern uint8_t gCtrlMode_Status; //0 auto 1 remote 2 stop 3 platform
extern uint8_t gNavagationSwitch;

#ifndef GLOBAL_GPS_POINT
#define GLOBAL_GPS_POINT
struct gGPSPoint
{
    double latitude = 0.0;
    double longitude = 0.0;
    double height = 0.0;
};
#endif
extern QVector<gGPSPoint> gPOIPoints;

using org::jeecg::defsDetails::grpc::Empty; ///< other message
using org::jeecg::defsDetails::grpc::GPSPoint;
using org::jeecg::defsDetails::grpc::MapPoint;

using org::jeecg::defsDetails::grpc::ShiftStatus; ///< other enum
using org::jeecg::defsDetails::grpc::CtrlMode;

DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
{
    stub_ = DataExchange::NewStub(channel);

    dec_yaml(stryamlpath);

    QObject::connect(this,&DataExchangeClient::destination_Recieved,this,&DataExchangeClient::destination_Recieved_Slot);
    QObject::connect(this,&DataExchangeClient::path_Updated,this,&DataExchangeClient::path_Updated_Slot);
    QObject::connect(&destinationRefreshedTimer,&QTimer::timeout,this,&DataExchangeClient::destination_Refreshed_Timeout);

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

    ModuleFun funupdate = std::bind(&DataExchangeClient::ListenFrontPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmPicFront.mpa = iv::modulecomm::RegisterRecvPlus(shmPicFront.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::ListenRearPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmPicRear.mpa = iv::modulecomm::RegisterRecvPlus(shmPicRear.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::ListenLeftPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmPicLeft.mpa = iv::modulecomm::RegisterRecvPlus(shmPicLeft.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::ListenRightPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmPicRight.mpa = iv::modulecomm::RegisterRecvPlus(shmPicRight.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::ListenChassisMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmChassis.mpa = iv::modulecomm::RegisterRecvPlus(shmChassis.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::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(&DataExchangeClient::ListenPlatformFeedbackMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmPlatformFeedback.mpa = iv::modulecomm::RegisterRecvPlus(shmPlatformFeedback.mstrmsgname,funupdate);
    funupdate = std::bind(&DataExchangeClient::ListenTraceMapMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    shmTraceMap.mpa = iv::modulecomm::RegisterRecvPlus(shmTraceMap.mstrmsgname,funupdate);

    id = gstrid;
    plateNumber = gstrplateNumber;
    switch (std::atoi(gvehicleType.c_str())) {
    case 0:
        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
        break;
    case 1:
        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::GUIDE;
        break;
    case 2:
        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::CLEAN;
        break;
    default:
        vehicleType = org::jeecg::defsDetails::grpc::VehicleClass::RUN_ERRANDS;
        break;
    }
}

DataExchangeClient::~DataExchangeClient(void)
{
    if(shmXodrRequest.mpa != nullptr)iv::modulecomm::Unregister(shmXodrRequest.mpa);
    if(shmPicFront.mpa != nullptr)iv::modulecomm::Unregister(shmPicFront.mpa);
    if(shmPicRear.mpa != nullptr)iv::modulecomm::Unregister(shmPicRear.mpa);
    if(shmPicLeft.mpa != nullptr)iv::modulecomm::Unregister(shmPicLeft.mpa);
    if(shmPicRight.mpa != nullptr)iv::modulecomm::Unregister(shmPicRight.mpa);
    if(shmChassis.mpa != nullptr)iv::modulecomm::Unregister(shmChassis.mpa);
    if(shmGPSIMU.mpa != nullptr)iv::modulecomm::Unregister(shmGPSIMU.mpa);
    if(shmPlatformFeedback.mpa != nullptr)iv::modulecomm::Unregister(shmPlatformFeedback.mpa);
    if(shmTraceMap.mpa != nullptr)iv::modulecomm::Unregister(shmTraceMap.mpa);
    requestInterruption();
    while(this->isFinished() == false);
}

void DataExchangeClient::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["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["chassis"])
    {
        if(config["chassis"]["msgname"]&&config["chassis"]["buffersize"]&&config["chassis"]["buffercount"])
        {
            strmsgname = config["chassis"]["msgname"].as<std::string>();
            strncpy(shmChassis.mstrmsgname,strmsgname.data(),255);
            shmChassis.mnBufferSize = config["chassis"]["buffersize"].as<int>();
            shmChassis.mnBufferCount = config["chassis"]["buffercount"].as<int>();
            std::cout << "chassis:" << shmChassis.mstrmsgname << "," << shmChassis.mnBufferSize << "," << shmChassis.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "chassis";
        strncpy(shmChassis.mstrmsgname,strmsgname.data(),255);
        shmChassis.mnBufferSize = 10000;
        shmChassis.mnBufferCount = 1;
    }

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

    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["trace_map"])
    {
        if(config["trace_map"]["msgname"]&&config["trace_map"]["buffersize"]&&config["trace_map"]["buffercount"])
        {
            strmsgname = config["trace_map"]["msgname"].as<std::string>();
            strncpy(shmTraceMap.mstrmsgname,strmsgname.data(),255);
            shmTraceMap.mnBufferSize = config["trace_map"]["buffersize"].as<int>();
            shmTraceMap.mnBufferCount = config["trace_map"]["buffercount"].as<int>();
            std::cout << "trace_map:" << shmTraceMap.mstrmsgname << "," << shmTraceMap.mnBufferSize << "," << shmTraceMap.mnBufferCount << std::endl;
        }
    }
    else
    {
        strmsgname = "simpletrace";
        strncpy(shmTraceMap.mstrmsgname,strmsgname.data(),255);
        shmTraceMap.mnBufferSize = 100000;
        shmTraceMap.mnBufferCount = 1;
    }

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

    return;
}

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

    gMutex_ImageFront.lock();
    cameraImageFront.clear();
    if(statusFeedback == VehicleStatus::STATUS_REMOTE && gShift_Status == 6)
        cameraImageFront.append(xdata.picdata().data(),xdata.picdata().size());
    else if(statusFeedback == VehicleStatus::STATUS_AUTO || statusFeedback == VehicleStatus::STATUS_EMERGENCY_STOP)
        cameraImageFront.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageFront.unlock();
}

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

    gMutex_ImageRear.lock();
    cameraImageRear.clear();
    if(statusFeedback == VehicleStatus::STATUS_REMOTE && gShift_Status == 4)
        cameraImageRear.append(xdata.picdata().data(),xdata.picdata().size());
    else if(statusFeedback == VehicleStatus::STATUS_AUTO || statusFeedback == VehicleStatus::STATUS_EMERGENCY_STOP)
        cameraImageRear.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageRear.unlock();
}

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

    gMutex_ImageLeft.lock();
    cameraImageLeft.clear();
    if(statusFeedback == VehicleStatus::STATUS_REMOTE && gShift_Status == 5)
        cameraImageLeft.append(xdata.picdata().data(),xdata.picdata().size());
    else if(statusFeedback == VehicleStatus::STATUS_AUTO || statusFeedback == VehicleStatus::STATUS_EMERGENCY_STOP)
        cameraImageLeft.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageLeft.unlock();
}

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

    gMutex_ImageRight.lock();
    cameraImageRight.clear();
    if(statusFeedback == VehicleStatus::STATUS_REMOTE && gShift_Status == 5)
        cameraImageRight.append(xdata.picdata().data(),xdata.picdata().size());
    else if(statusFeedback == VehicleStatus::STATUS_AUTO || statusFeedback == VehicleStatus::STATUS_EMERGENCY_STOP)
        cameraImageRight.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageRight.unlock();
}

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

    gMutex_Chassis.lock();
    SOC = xdata.soc();
    gMutex_Chassis.unlock();
//    std::cout<<SOC<<std::endl;
}

void DataExchangeClient::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 GPS_Speed = sqrt(xdata.ve()*xdata.ve()+xdata.vd()*xdata.vd()+xdata.vn()*xdata.vn());
//    if (GPS_Speed < 0.01)GPS_Speed = 0.0;

    gMutex_GPSIMU.lock();
    GPSRTKStatus = xdata.rtk_state();
    positionFeedback.set_latitude(xdata.lat());
    positionFeedback.set_longitude(xdata.lon());
    positionFeedback.set_height(xdata.height());
    pitch = xdata.pitch();
    roll = xdata.roll();
    heading = xdata.heading();
    speed = xdata.speed();
//    std::cout<<"speed: "<<speed<<std::endl;
    if(speed < 0.01)speed = 0.0;
    gMutex_GPSIMU.unlock();
//    std::cout<<heading<<std::endl;
}

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

    gMutex_PlatformFeedback.lock();
    switch (xdata.typefeedback()) {
    case iv::platformFeedback::ctrlType::platformFeedback_ctrlType_AUTO:
        statusFeedback = VehicleStatus::STATUS_AUTO;
        modeFeedback = CtrlMode::CMD_AUTO;
        break;
    case iv::platformFeedback::ctrlType::platformFeedback_ctrlType_REMOTE:
        if(gCtrlMode_Status == 1) //remote
        {
            statusFeedback = VehicleStatus::STATUS_REMOTE;
            modeFeedback = CtrlMode::CMD_REMOTE;
        }
        else if(gCtrlMode_Status == 3) //platform
        {
            statusFeedback = VehicleStatus::STATUS_REMOTE;
            modeFeedback = CtrlMode::CMD_CLOUD_PLATFORM;
        }
        break;
    case iv::platformFeedback::ctrlType::platformFeedback_ctrlType_STOP:
        statusFeedback = VehicleStatus::STATUS_EMERGENCY_STOP;
        modeFeedback = CtrlMode::CMD_EMERGENCY_STOP;
        break;
    default:
        break;
    }

    switch (xdata.shift()) {
    case 0: //0N 1D 2R 3P
        shiftFeedback = ShiftStatus::SHIFT_NEUTRAL; //3P 4R 5N 6D
        gShift_Status = 5;
        break;
    case 1: //0N 1D 2R 3P
        shiftFeedback = ShiftStatus::SHIFT_DRIVE;
        gShift_Status = 6;
        break;
    case 2: //0N 1D 2R 3P
        shiftFeedback = ShiftStatus::SHIFT_REVERSE;
        gShift_Status = 4;
        break;
    case 3: //0N 1D 2R 3P
        shiftFeedback = ShiftStatus::SHIFT_PARKING;
        gShift_Status = 3;
        break;
    default:
        break;
    }

    throttleFeedback = xdata.throttle();
    brakeFeedback = xdata.brake();
    steeringWheelAngleFeedback = (-1) * xdata.steeringwheelangle();
    gMutex_PlatformFeedback.unlock();
//    std::cout<<throttleFeedback<<","<<brakeFeedback<<","<<steeringWheelAngleFeedback<<","<<shiftFeedback<<","<<modeFeedback<<std::endl;
}

void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
// recive trance map
    int npoint = nSize/sizeof(iv::simpletrace);
    if(npoint < 1)
    {
        std::cout<<"simple trace is very small"<<std::endl;
        return;
    }
    std::vector<iv::simpletrace> simpletrace;
    simpletrace.clear();
    simpletrace.resize(npoint);
    memcpy(simpletrace.data(),strdata,npoint*sizeof(iv::simpletrace));

    org::jeecg::defsDetails::grpc::MapPoint onePoint;
    pathPoints.clear();
    for(int i = 0;i < npoint;i++)
    {
        onePoint.set_index(i);
        onePoint.mutable_mappoint()->set_latitude(simpletrace[i].gps_lat);
        onePoint.mutable_mappoint()->set_longitude(simpletrace[i].gps_lng);
        pathPoints.append(onePoint);
//        std::cout<<pathPoints.at(i).index()<<std::endl;
    }
    emit setFSMDestination(pathPoints.at(pathPoints.size()-1).mappoint().latitude(),pathPoints.at(pathPoints.size()-1).mappoint().longitude());
    std::cout<<"get a tracemap"<<std::endl;

    totalPathLength = 0.0;
    double localPositionX = 0.0;
    double localPositionNextX = 0.0;
    double localPositionY = 0.0;
    double localPositionNextY = 0.0;
    for(int i = 0;i < npoint - 1;i++)
    {
        GaussProjCal(simpletrace[i].gps_lng,simpletrace[i].gps_lat,&localPositionX,&localPositionY);
        GaussProjCal(simpletrace[i+1].gps_lng,simpletrace[i+1].gps_lat,&localPositionNextX,&localPositionNextY);
        double deltaX = localPositionNextX - localPositionX;
        double deltaY = localPositionNextY - localPositionY;
        totalPathLength += sqrt(deltaX*deltaX + deltaY*deltaY);
    }
    std::cout<<"point number: "<<npoint<<",total length: "<<totalPathLength<<std::endl;

    arrivedTime = totalPathLength/gspeedWantAvg;
    waitTime = gwaitTime;
    if(destinationRefreshedTimer.isActive() == true)
        emit path_Updated();
}

std::string DataExchangeClient::uploadVehicleInfo(void)
{

    // Data we are sending to the server.
    UplinkRequest request;
    request.set_id(id);
    request.set_timestamp(timeStamp);

    gMutex_Chassis.lock();
    request.set_soc(SOC);
    gMutex_Chassis.unlock();

    gMutex_PlatformFeedback.lock();
    request.set_statusfeedback(statusFeedback);//statusFeedback
    request.set_modefeedback(modeFeedback);//modeFeedback
    request.set_shiftfeedback(shiftFeedback);
    request.set_steeringwheelanglefeedback(steeringWheelAngleFeedback);
    request.set_throttlefeedback(throttleFeedback);
    request.set_brakefeedback(brakeFeedback);
    gMutex_PlatformFeedback.unlock();

    gMutex_GPSIMU.lock();
    request.set_mileage(mileage);
    request.set_speed(speed);
    request.set_gpsrtkstatus(GPSRTKStatus);
    request.mutable_positionfeedback()->CopyFrom(positionFeedback);
    request.set_pitch(pitch);
    request.set_roll(roll);
    request.set_heading(heading);
    gMutex_GPSIMU.unlock();
//  std::cout<<std::setprecision(12)<<"vehicle position: "<<positionFeedback.latitude()<<","<<positionFeedback.longitude()<<std::endl;

//    gMutex_ImageFront.lock();
//    request.set_cameraimagefront(cameraImageFront.data(),cameraImageFront.size());
//    gMutex_ImageFront.unlock();
//    gMutex_ImageRear.lock();
//    request.set_cameraimagerear(cameraImageRear.data(),cameraImageRear.size());
//    gMutex_ImageRear.unlock();
//    gMutex_ImageLeft.lock();
//    request.set_cameraimageleft(cameraImageLeft.data(),cameraImageLeft.size());
//    gMutex_ImageLeft.unlock();
//    gMutex_ImageRight.lock();
//    request.set_cameraimageright(cameraImageRight.data(),cameraImageRight.size());
//    gMutex_ImageRight.unlock();

    request.set_sensorstatusgpsimu(sensorStatusGPSIMU);
    request.set_sensorstatuslidar(sensorStatusLidar);
    request.set_sensorstatusradar(sensorStatusRadar);
    request.set_sensorstatuscamfront(sensorStatusCamFront);
    request.set_sensorstatuscamrear(sensorStatusCamRear);
    request.set_sensorstatuscamleft(sensorStatusCamLeft);
    request.set_sensorstatuscamright(sensorStatusCamRight);
    if(isArrived == 1)
    {
        if(gNavagationSwitch == 0)
            request.set_isarrived(0);
        if(gNavagationSwitch == 1)
            request.set_isarrived(isArrived);
    }
    else
        request.set_isarrived(isArrived);
//    std::cout<<"isarrived: "<<isArrived<<std::endl;
    request.set_platenumber(plateNumber);
    request.set_usestatusfeedback(useStatus);
    request.set_remainpathlength(remainPathLength);
    request.set_classfeedback(vehicleType);

    // Container for the data we expect from the server.
    ResponseMessage 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 = 2;
    timespec.tv_nsec = 0;
    timespec.clock_type = GPR_TIMESPAN;
    context.set_deadline(timespec);

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

    // Act upon its status.
    if (status.ok()) {
        destinationPosition.CopyFrom(reply.destinationposition());
        emit destination_Recieved();
//        std::cout<<std::setprecision(12)<<"lat:"<<reply.destinationposition().latitude()<<"lon:"<<reply.destinationposition().longitude()<<"height:"<<reply.destinationposition().height()<<std::endl;
        return "uploadVehicleInfo 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 "uploadVehicleInfo RPC failed";
    }
}

std::string DataExchangeClient::uploadPath(void)
{
    // Data we are sending to the server.
    UploadPathRequest request;
    request.set_id(id);
    request.set_patrolpathid(patrolPathID);
    for(int i = 0;i < pathPoints.size();i++)
    {
        request.add_pathpoints();
        request.mutable_pathpoints(i)->operator =(pathPoints.at(i));
    }
    request.set_arrivedtime(arrivedTime);
    request.set_waittime(waitTime);
    request.set_totalpathlength(totalPathLength);

    // 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_ -> uploadPath(&context,request,&reply);

    // Act upon its status.
    if (status.ok()) {
        if(reply.id() == gstrid)
        {
            std::cout<<"Path uploaded by car id:"<<reply.id()<<std::endl;
            emit uploadPath_Finished(patrolPathID);
        }
        return "uploadPath 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 "uploadPath RPC failed";
    }
}

void DataExchangeClient::updateData(uint64_t timeInterval_ms)
{
    org::jeecg::defsDetails::grpc::GPSPoint tempPosition;

    timeStamp = QDateTime::currentMSecsSinceEpoch();
//    SOC = 87.5;
//    statusFeedback = VehicleStatus::STATUS_REMOTE;
    gMutex_GPSIMU.lock();
    mileage += (speed * timeInterval_ms/1000.0)/1000.0; // kilometer
    tempPosition.CopyFrom(positionFeedback);
//    std::cout<<std::setprecision(12)<<tempPosition.latitude()<<","<<tempPosition.longitude()<<std::endl;
    gMutex_GPSIMU.unlock();
//    speed = 0.0; // m/s
//    shiftFeedback = ShiftStatus::SHIFT_DRIVE;
//    steeringWheelAngleFeedback = 1.23; //+/-540 degree
//    throttleFeedback = 0.12;
//    brakeFeedback = 0.34;
//    GPSRTKStatus = 6; //GPS-RTK status 0-6 6 is best
//    positionFeedback.set_latitude(39.0666552);
//    positionFeedback.set_longitude(117.3540963);
//    positionFeedback.set_height(0.84);
//    pitch = 0.03;
//    roll = 0.02;
//    heading = 198.4;

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

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

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

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

    sensorStatusGPSIMU = false; //0 GPS-IMU ok 1 GPS-IMU error
    sensorStatusLidar = false;
    sensorStatusRadar = false;
    sensorStatusCamFront = false;
    sensorStatusCamRear = false;
    sensorStatusCamLeft = false;
    sensorStatusCamRight = false;

//    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
    if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
    {
        destinationSaved.Clear();
        remainPathLength = 0.0;
        isArrived = 0;
    }

    remainPathLength -= (speed * timeInterval_ms/1000.0);
    if(remainPathLength > 0.0)std::cout<<"remainLength: "<<remainPathLength<<std::endl;
    if(remainPathLength <= 0.0)
        remainPathLength = 0.0;

    if(isArrived == 1)
    {
        double localPositionX = 0.0;
        double localPositionNextX = 0.0;
        double localPositionY = 0.0;
        double localPositionNextY = 0.0;
        GaussProjCal(tempPosition.longitude(),tempPosition.latitude(),&localPositionX,&localPositionY);
        GaussProjCal(destinationSaved.longitude(),destinationSaved.latitude(),&localPositionNextX,&localPositionNextY);
        double deltaX = localPositionNextX - localPositionX;
        double deltaY = localPositionNextY - localPositionY;
        double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
//        if(distance < 20.0)
//            std::cout<<"distance: "<<distance<<std::endl;
        if(distance < 1.5)
        {
            isArrived = 2;
            remainPathLength = 0.0;
            destinationSaved.Clear();
            emit setMissionFinished();
            std::cout<<"destination has arrived"<<std::endl;
        }
    }
//    std::cout<<"is arrived: "<<isArrived<<std::endl;
}

void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points)
{
    id = gstrid;
    patrolPathID = pathID;
    pathPoints.clear();
    for(int i = 0;i < points.size();i++)
    {
        pathPoints.append(points.value(i));
//        std::cout<<pathPoints.at(i).index()<<std::endl;
    }
}

void DataExchangeClient::run()
{
    QTime xTime;
    xTime.start();

    QFile mileageFile;
    mileageFile.setFileName("./mileage.txt");
    int fileWriteTime = xTime.elapsed();
    if(mileageFile.exists())
    {
        if(mileageFile.open(QIODevice::ReadOnly | QIODevice::Text))
        {
            mileage = mileageFile.readAll().toDouble();
            std::cout<<"file open success. mileage is "<<mileage<<std::endl;
        }
        mileageFile.close();
    }
    else
    {
        if(mileageFile.open(QIODevice::WriteOnly | QIODevice::Text))
        {
            QString strMileage = QString::number(mileage,'f',4);
            mileageFile.write(strMileage.toUtf8());
            std::cout<<"file not exist. mileage has been cleared. "<<mileage<<std::endl;
        }
        mileageFile.flush();
        mileageFile.close();
    }

    int lastTime = xTime.elapsed();
    int realLastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstruploadInterval.c_str());
    while (!QThread::isInterruptionRequested())
    {
        if(abs(xTime.elapsed() - lastTime)>=interval)
        {
            uint64_t realInterval = abs(xTime.elapsed() - realLastTime);
            realLastTime = xTime.elapsed();
            updateData(realInterval);
            std::string reply = uploadVehicleInfo();
//            std::cout<< reply <<std::endl;
//            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
            lastTime = xTime.elapsed();
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::microseconds(500));///< sleep 0.5ms
        }

        if(abs(xTime.elapsed() - fileWriteTime) >= 2500)
        {
            if(mileageFile.open(QIODevice::WriteOnly | QIODevice::Text))
            {
                QString strMileage = QString::number(mileage,'f',4);
                mileageFile.write(strMileage.toUtf8());
            }
            mileageFile.flush();
            mileageFile.close();
            fileWriteTime = xTime.elapsed();
        }
        else
        {
            std::this_thread::sleep_for(std::chrono::microseconds(500));///< sleep 0.5ms
        }
    }
}

void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
{
        isArrived = 0;
        remainPathLength = 0.0;
        destinationSaved.Clear();
        patrolPathID = pathID;

        xodrobj xodrDest;
        xodrDest.flon = gPOIPoints.at(1).longitude;
        xodrDest.flat = gPOIPoints.at(1).latitude;
        xodrDest.lane = 1;
        std::cout<<"destination: "<<std::setprecision(12)<<gPOIPoints.at(1).longitude<<","<<gPOIPoints.at(1).latitude<<std::endl;

        iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
//        std::cout<<"modulesend xodr request"<<std::endl;
        if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
        std::cout<<"patrol path calculating"<<std::endl;

//    QFile mapfile;
//    mapfile.setFileName("/home/nvidia/Documents/path2.txt");
//    QVector<org::jeecg::defsDetails::grpc::MapPoint> somePoints;
//    if(mapfile.open(QIODevice::ReadOnly | QIODevice::Text))
//    {
//        while(!mapfile.atEnd())
//        {
//            QByteArray line = mapfile.readLine();
//            QString map_str(line);
//            QStringList oneline = map_str.split(",");
//            org::jeecg::defsDetails::grpc::MapPoint onePoint;
//            onePoint.set_index(oneline.at(0).toInt());
//            onePoint.mutable_mappoint()->set_longitude(oneline.at(1).toDouble());
//            onePoint.mutable_mappoint()->set_latitude(oneline.at(2).toDouble());
//            onePoint.mutable_mappoint()->set_height(oneline.at(3).toDouble());
//            somePoints.append(onePoint);
//        }
//    }
//    updatePath(pathID,somePoints);
//    uploadPath();
}

void DataExchangeClient::destination_Recieved_Slot(void)
{
    if(fabs(destinationPosition.latitude()) > 0.000001 \
            && fabs(destinationPosition.longitude()) > 0.000001)
    {

    }
}

void DataExchangeClient::path_Updated_Slot(void)
{
    if(destinationRefreshedTimer.isActive() == true)
        destinationRefreshedTimer.stop();
    destinationSaved.set_latitude(pathPoints.at(pathPoints.size()-1).mappoint().latitude());
    destinationSaved.set_longitude(pathPoints.at(pathPoints.size()-1).mappoint().longitude());
    destinationSaved.set_height(0.0);
    remainPathLength = totalPathLength;
    isArrived = 1; ///< 0 no path 1 not arrived 2 arrived
    uploadPath();
}

void DataExchangeClient::destination_Refreshed_Timeout(void)
{
    if(destinationRefreshedTimer.isActive() == true)
        destinationRefreshedTimer.stop();
    destinationSaved.Clear();
    emit setFSMDestination(destinationSaved.latitude(),destinationSaved.longitude());
    remainPathLength = 0.0;
    isArrived = 0;
    emit uploadPath_Finished("");
    std::cout<<"path plan timeout."<<std::endl;
}

void DataExchangeClient::useStatusChanged_Slot(bool status)
{
    if(status)
        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
    else
        useStatus = org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING;
}