modularization/src/controller/controller_hapo/main.cpp

#include <QCoreApplication>

#include <QTime>

#include <QMutex>

#include "control/control_status.h"
#include "control/controller.h"

#include "xmlparam.h"
#include "modulecomm.h"
#include "ivbacktrace.h"
#include "ivversion.h"

#include "canmsg.pb.h"
#include "decition.pb.h"
#include "chassis.pb.h"

#include "torquebrake.h"

#include <thread>

#include "remotectrl.pb.h"
#include "platform_feedback.pb.h"

void * gpacansend;
void * gpadecition;
void * gpachassis;
void * gparemote;
void * gpaPaltformFeedback;

std::string gstrmemdecition;
std::string gstrmemcansend;
std::string gstrmemchassis;
std::string gstrmemremote; //Remote Ctrl
std::string gstrmemPaltformFeedback;

bool gbSendRun = true;

bool gbChassisEPS = false;

iv::brain::decition gdecition_def;
iv::brain::decition gdecition;

bool gbAllowRemote = false;   //Default, Not Allow Remote

bool gbAutoDriving = true; //if true, Auto Driving, false, remote controll

iv::brain::decition gdecition_remote;
iv::platformFeedback gPlatformFeedback;

qint64 gLastRemoteTime = 0;

QTime gTime;
int gnLastSendTime = 0;
int gnLastRecvDecTime = -1000;
int gnDecitionNum = 0; //when is zero,send default;
const int gnDecitionNumMax = 100;
int gnIndex = 0;

boost::shared_ptr<iv::control::Controller> gcontroller;	//实际车辆控制器

static QMutex gMutex;
//void executeDecition(const iv::decition::Decition decition)
//{

//    std::cout<<"acc is"<<decition->accelerator<<" ang is "<<decition->wheel_angle<<std::endl;

//    controller_->control_wheel(decition->wheel_angle);
//	controller_->control_accelerate(decition->accelerator);


//        if(!decition->leftlamp && !decition->rightlamp){
//             ServiceControlStatus.set_turnsignals_control(0,0);
//        }else if(decition->leftlamp){
//        ServiceControlStatus.set_turnsignals_control(decition->leftlamp,0);
//        }else if(decition->rightlamp){
//        ServiceControlStatus.set_turnsignals_control(0,decition->rightlamp);
//        }
//}
//	controller_->control_accelerate(decition->accelerator);

void executeDecition(const iv::brain::decition decition)
{

//    std::cout<<"acc is "<<decition.torque()<<" ang is "<<decition.wheelangle()<<std::endl;

    gcontroller->control_acc_en(true);
    gcontroller->control_angle_enable(true);
    gcontroller->control_gear_en(true);
    gcontroller->control_speed_limit(30);

    gcontroller->control_wheel(decition.wheelangle());
    gcontroller->control_angle_speed(decition.wheelspeed());
    gcontroller->control_torque(decition.torque());

    gcontroller->control_brake(decition.brake());
    gcontroller->control_gear(decition.gear());

    gcontroller->control_handBrake(decition.handbrake());
    gcontroller->control_mode(decition.mode());
    //   gcontroller->control_mode(1);
    gcontroller->control_near_light(decition.dippedlight());


    //gcontroller->control_air_enable(decition.air_enable());
    //gcontroller->control_air_on(decition.air_on());
    //gcontroller->control_air_temp(decition.air_temp());
    //gcontroller->control_air_mode(decition.air_mode());
    //gcontroller->control_wind_level(decition.wind_level());
    gcontroller->control_roof_light(decition.roof_light());
    gcontroller->control_home_light(decition.home_light());
    //gcontroller->control_air_worktime(decition.air_worktime());
    //gcontroller->control_air_offtime(decition.air_offtime());
    gcontroller->control_turnsignals(decition.leftlamp(),decition.rightlamp());
    gcontroller->control_door(decition.door());

#ifdef TORQUEBRAKETEST
    if(gnothavetb < 10)
    {
        iv::controller::torquebrake xtb;
        gMutextb.lock();
        xtb.CopyFrom(gtb);
        gMutextb.unlock();
        if(xtb.enable())
        {
            gcontroller->control_torque(xtb.torque());
            gcontroller->control_brake(xtb.brake());
            qDebug("use tb value torque is %f brake is %f",xtb.torque(),xtb.brake());
        }
        else
        {
//            qDebug("torquebrake not enable.");
        }
        gnothavetb++;
    }

#endif

    /*if(decition.has_door())
    {
        if(decition.door())
        {
            gcontroller->control_door(2);
        }else{
            gcontroller->control_door(3);
        }
    }*/


//qDebug("door is %d",decition.door());

    gcontroller->cmd_checksum(0x10);
    gcontroller->cmd_checksum(0x11);
    gcontroller->cmd_checksum(0x12);

    switch (decition.gear()) {
    case 0:
        gPlatformFeedback.set_shift(3);  //0N 1D 2R 3P
        break;
    case 1:
        gPlatformFeedback.set_shift(1);  //0N 1D 2R 3P
        break;
    case 2:
        gPlatformFeedback.set_shift(2);  //0N 1D 2R 3P
        break;
    case 3:
        gPlatformFeedback.set_shift(0);  //0N 1D 2R 3P
        break;
    default:
        break;
    }
    gPlatformFeedback.set_throttle(decition.torque());
    gPlatformFeedback.set_brake(decition.brake());
    gPlatformFeedback.set_steeringwheelangle(decition.wheelangle());

    //    qDebug("dangwei is %d mode is %d",decition.gear(),decition.mode());
}

void ListenRemotectrl(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    qint64 oldtime;
    iv::brain::decition xdecition;

    iv::remotectrl xrc;

    if(!xrc.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenRemotectrl parse error."<<std::endl;
        return;
    }

    if(xrc.ntype() == iv::remotectrl_CtrlType_AUTO)
    {
        gbAutoDriving = true;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_AUTO);
    }
    else if(xrc.ntype() == iv::remotectrl_CtrlType_STOP)
    {
        gbAutoDriving = false;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_STOP);
        xdecition.set_torque(0.0);
        xdecition.set_brake(100.0);
        xdecition.set_wheelangle(0.0);
        xdecition.set_gear(1); //0P 1D 2R 3N
        xdecition.set_handbrake(0);
        xdecition.set_mode(1);
        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();
    }
    else
    {
        std::cout<<"  shift "<<xrc.shift()<<" acc "<<xrc.acc()<<" brake: "<<xrc.brake()<<" wheel: "<<xrc.wheel()<<std::endl;
        gbAutoDriving = false;
        gPlatformFeedback.set_typefeedback(iv::platformFeedback::ctrlType::platformFeedback_ctrlType_REMOTE);
        xdecition.set_torque(xrc.acc()*0.08);
        xdecition.set_brake(xrc.brake());
        xdecition.set_wheelangle(xrc.wheel()*5.5f);
        if(xrc.shift() > 0) //D shift
            xdecition.set_gear(4); //0P 1D 2R 3N
        else if(xrc.shift() < 0)
            xdecition.set_gear(2);
//        else
//            xdecition.set_gear(3);
        xdecition.set_handbrake(0);
        xdecition.set_mode(1);
        gMutex.lock();
        gdecition_remote.CopyFrom(xdecition);
        gMutex.unlock();
        gLastRemoteTime = QDateTime::currentMSecsSinceEpoch();
    }

}

void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    iv::chassis xchassis;
    static int ncount = 0;
    if(!xchassis.ParseFromArray(strdata,nSize))
    {
        std::cout<<"iv::decition::BrainDecition::UpdateChassis ParseFrom Array Error."<<std::endl;
        return;
    }

    if(xchassis.epsmode() == 0)
    {
        gbChassisEPS = true;
    }


}

void ListenDeciton(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    qint64 oldtime;
    iv::brain::decition xdecition;

    if(!xdecition.ParseFromArray(strdata,nSize))
    {
        std::cout<<"ListenDecition parse error."<<std::endl;
        return;
    }

    if(xdecition.gear() != 4)
    {
        qDebug("not D");
    }
    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %ld diff is %ld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
    oldtime = QDateTime::currentMSecsSinceEpoch();
    gMutex.lock();
    gdecition.CopyFrom(xdecition);
    gMutex.unlock();
    gnDecitionNum = gnDecitionNumMax;
    gbChassisEPS = false;

}

void ExecSend()
{
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
    unsigned char * strp = (unsigned char *)&(ServiceControlStatus.command10.byte[0]);
    //    qDebug("%02x %02x %02x %02x %02x %02x %02x %02x",strp[0]
    //            ,strp[1],strp[2],strp[3],strp[4],strp[5],strp[6],strp[7]);
    xraw.set_id(ServiceControlStatus.command_ID10);
    xraw.set_data(ServiceControlStatus.command10.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw = xmsg.add_rawmsg();
    pxraw->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(ServiceControlStatus.command_ID11);
    xraw.set_data(ServiceControlStatus.command11.byte,8);
    int a = ServiceControlStatus.command11.byte[5]&0x0f;
    if(a != 0x04)
    {
        qDebug("not D.");
    }
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw1 = xmsg.add_rawmsg();
    pxraw1->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    xraw.set_id(ServiceControlStatus.command_ID12);
    xraw.set_data(ServiceControlStatus.command12.byte,8);
    xraw.set_bext(false);
    xraw.set_bremote(false);
    xraw.set_len(8);
    iv::can::canraw * pxraw2 = xmsg.add_rawmsg();
    pxraw2->CopyFrom(xraw);
    xmsg.set_channel(0);
    xmsg.set_index(gnIndex);

    gnIndex++;
    xmsg.set_mstime(QDateTime::currentMSecsSinceEpoch());
    int ndatasize = xmsg.ByteSize();
    char * strser = new char[ndatasize];
    std::shared_ptr<char> pstrser;
    pstrser.reset(strser);
    if(xmsg.SerializeToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }

    int ndatasize_pf = gPlatformFeedback.ByteSize();
    char * str_pf = new char[ndatasize_pf];
    std::shared_ptr<char> pstr;pstr.reset(str_pf);
    if(!gPlatformFeedback.SerializeToArray(str_pf,ndatasize_pf))
    {
        std::cout<<"MainWindow::on_horizontalSlider_valueChanged serialize error."<<std::endl;
        return;
    }
    iv::modulecomm::ModuleSendMsg(gpaPaltformFeedback,str_pf,ndatasize_pf);
}

void initial()
{
    //    ServiceControlStatus.command.byte[0] = 0;
    //    ServiceControlStatus.command.byte[1] = 0;
    //    ServiceControlStatus.command.byte[2] = 0;
    //    ServiceControlStatus.command.byte[3] = 0;
    //    ServiceControlStatus.command.byte[4] = 0;
    //    ServiceControlStatus.command.byte[5] = 0;
    //    ServiceControlStatus.command.byte[6] = 0;
    //    ServiceControlStatus.command.byte[7] = 0;
}

void sendthread()
{
    initial();
    iv::brain::decition xdecition;
    while(gbSendRun)
    {
        if(gbAutoDriving)
        {
            if(gnDecitionNum <= 0)
            {
                xdecition.CopyFrom(gdecition_def);
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition);
                gMutex.unlock();
                gnDecitionNum--;
            }
        }
        else
        {
            if((QDateTime::currentMSecsSinceEpoch() - gLastRemoteTime)> 1000)
            {
                xdecition.CopyFrom(gdecition_def);
            }
            else
            {
                gMutex.lock();
                xdecition.CopyFrom(gdecition_remote);
                gMutex.unlock();
            }
        }
        executeDecition(xdecition);
        if(gbChassisEPS == false) ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_hapo");
    QCoreApplication a(argc, argv);

    QString strpath = QCoreApplication::applicationDirPath();

    if(argc < 2)
        strpath = strpath + "/controller_vv7.xml";
    else
        strpath = argv[1];
    std::cout<<strpath.toStdString()<<std::endl;

    gdecition_def.set_accelerator(-0.5);
    //    gdecition_def.set_brake(1.0);
    gdecition_def.set_brake(20);   //apollo_fu 20200417
    gdecition_def.set_torque(0);
    // gdecition_def.set_rightlamp(true);
    //  gdecition_def.set_doublespark(true);
    gdecition_def.set_wheelangle(0);
    gdecition_def.set_mode(1);
    gdecition_def.set_grade(1);
    gdecition_def.set_gear(1);
    gdecition_def.set_engine(0);
    //    gdecition_def.set_gear(0);
    gTime.start();

    gcontroller = boost::shared_ptr<iv::control::Controller>(new iv::control::Controller());

    iv::xmlparam::Xmlparam xp(strpath.toStdString());

    gstrmemcansend = xp.GetParam("cansend","cansend0");
    gstrmemdecition = xp.GetParam("dection","deciton");
    gstrmemchassis = xp.GetParam("chassismsgname","chassis");
    gstrmemremote =  xp.GetParam("remotectrl","remotectrl");
    gstrmemPaltformFeedback = xp.GetParam("paltformFeedback","platformFeedback");
    std::string strremote = xp.GetParam("allowremote","true");
    if(strremote == "true")
    {
        gbAllowRemote = true;
    }

    gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
    gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);
    if(gbAllowRemote)
    {
        gpaPaltformFeedback = iv::modulecomm::RegisterSend(gstrmemPaltformFeedback.data(),10000,1);
        gparemote = iv::modulecomm::RegisterRecv(gstrmemremote.data(),ListenRemotectrl);
    }

#ifdef TORQUEBRAKETEST
    EnableTorqueBrakeTest();
#endif

    std::thread xthread(sendthread);

    return a.exec();
}