modularization/src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

#include "vehicle_upload.h"

#include <QFile>
#include <QString>
#include <QStringList>

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

extern uint8_t gShift_Status;//3 p 4 r 5 n 6 d

namespace iv {
struct msgunit
{
    char mstrmsgname[256];
    int mnBufferSize = 10000;
    int mnBufferCount = 1;
    void * mpa;
    std::shared_ptr<char> mpstrmsgdata;
    int mndatasize = 0;
    bool mbRefresh = false;
    bool mbImportant = false;
    int mnkeeptime = 100;
};
}
extern iv::msgunit shmPicFront;
extern iv::msgunit shmPicRear;
extern iv::msgunit shmPicLeft;
extern iv::msgunit shmPicRight;

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 * gDataExchangeClient;

void ListenFrontData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    gDataExchangeClient->ListenFrontPicMsg(strdata,nSize);
}

void ListenRearData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    gDataExchangeClient->ListenRearPicMsg(strdata,nSize);
}

void ListenLeftData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    gDataExchangeClient->ListenLeftPicMsg(strdata,nSize);
}

void ListenRightData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    gDataExchangeClient->ListenRightPicMsg(strdata,nSize);
}

DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
{
    gDataExchangeClient = this;

    stub_ = DataExchange::NewStub(channel);

    shmPicFront.mpa = iv::modulecomm::RegisterRecv(shmPicFront.mstrmsgname,ListenFrontData);
    shmPicRear.mpa = iv::modulecomm::RegisterRecv(shmPicRear.mstrmsgname,ListenRearData);
    shmPicLeft.mpa = iv::modulecomm::RegisterRecv(shmPicLeft.mstrmsgname,ListenLeftData);
    shmPicRight.mpa = iv::modulecomm::RegisterRecv(shmPicRight.mstrmsgname,ListenRightData);
}

DataExchangeClient::~DataExchangeClient(void)
{

}

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

    gMutex_ImageFront.lock();
    cameraImageFront.clear();
    if(gShift_Status == 6)cameraImageFront.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageFront.unlock();
}

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

    gMutex_ImageRear.lock();
    cameraImageRear.clear();
    if(gShift_Status == 4)cameraImageRear.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageRear.unlock();
}

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

    gMutex_ImageLeft.lock();
    cameraImageLeft.clear();
    if(gShift_Status == 5)cameraImageLeft.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageLeft.unlock();
}

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

    gMutex_ImageRight.lock();
    cameraImageRight.clear();
    if(gShift_Status == 5)cameraImageRight.append(xdata.picdata().data(),xdata.picdata().size());
    gMutex_ImageRight.unlock();
}

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

    // Data we are sending to the server.
    UplinkRequest request;
    request.set_id(id);
    request.set_timestamp(timeStamp);
    request.set_soc(SOC);
    request.set_statusfeedback(statusFeedback);
    request.set_mileage(mileage);
    request.set_speed(speed);
    request.set_shiftfeedback(shiftFeedback);
    request.set_steeringwheelanglefeedback(steeringWheelAngleFeedback);
    request.set_throttlefeedback(throttleFeedback);
    request.set_brakefeedback(brakeFeedback);
    request.set_gpsrtkstatus(GPSRTKStatus);
    request.mutable_positionfeedback()->CopyFrom(positionFeedback);
    request.set_pitch(pitch);
    request.set_roll(roll);
    request.set_heading(heading);
    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);
    request.set_isarrived(isArrived);
    request.set_platenumber(plateNumber);
    request.set_modefeedback(modeFeedback);

    // 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());
//        std::cout<<"lat:"<<destinationPosition.latitude()<<"lon:"<<destinationPosition.longitude()<<"height:"<<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));
    }

    // 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(void)
{
    id = gstrid;
    timeStamp = QDateTime::currentMSecsSinceEpoch();
    SOC = 87.5;
    statusFeedback = VehicleStatus::STATUS_REMOTE;
    mileage = 123.45; // kilometer
    speed = 0.1; // 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;

    isArrived = 0; //0 no destination 1 not arrived 2 arrived

    plateNumber = gstrplateNumber;

    modeFeedback = CtrlMode::CMD_REMOTE; //mode Feedback
}

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();
    uint64_t lastTime = xTime.elapsed();
    uint64_t interval = std::atoi(gstruploadInterval.c_str());
    while (true)
    {
        if((xTime.elapsed() - lastTime)>=interval)
        {
            updateData();
            std::string reply = uploadVehicleInfo();
            std::cout<< reply <<std::endl;
            lastTime = xTime.elapsed();
        }
    }
}

void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
{
    std::cout<<"patrol path calculating"<<std::endl;
    QFile mapfile("/home/samuel/Documents/path1.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::ctrlMode_Changed_Slot(org::jeecg::defsDetails::grpc::CtrlMode ctrlMode)
{
    modeFeedback = ctrlMode;
}