modularization/src/controller/controller_problue/main.cpp

#include <QCoreApplication>

#include <QTime>

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

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

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

#include <thread>

void * gpacansend;
void * gpadecition;
void * gpachassis;

std::string gstrmemdecition;
std::string gstrmemcansend;
std::string gstrmemchassis;
bool gbSendRun = true;

bool gbChassisEPS = false;

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


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;	//实际车辆控制器


//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.accelerator()<<" ang is "<<decition.wheelangle()<<std::endl;


    ServiceControlStatus.set_brake(decition.brake());
    ServiceControlStatus.set_torque(decition.torque());
    ServiceControlStatus.set_angle_speed((char)decition.wheelspeed());
    ServiceControlStatus.set_wheel_angle(decition.wheelangle());
    ServiceControlStatus.set_dangwei(decition.gear());
    ServiceControlStatus.set_handBrake(decition.handbrake());     //add by fjk
    //ServiceControlStatus.set_brake_type(decition->brakeType);   //?
    ServiceControlStatus.set_door(decition.door());
    ServiceControlStatus.set_driveMode(decition.mode());
    ServiceControlStatus.set_dangwei_enable(true);
    ServiceControlStatus.set_angle_enable(true);
    ServiceControlStatus.set_brake_enable(true);  //add by fjk
//	ServiceControlStatus.set_turnsignals_control(decition->left, decition->right);  //add by fjk
    ServiceControlStatus.set_near_light(0);
    ServiceControlStatus.set_far_light(0);
    ServiceControlStatus.set_speaker(0);
    if(decition.leftlamp()){
        ServiceControlStatus.set_turn_light(1);
    }else if(decition.rightlamp()){
        ServiceControlStatus.set_turn_light(2);
    }else {
        ServiceControlStatus.set_turn_light(0);
    }

//    gcontroller->control_wheel(decition.wheelangle());
//    //	controller_->control_accelerate(decition->accelerator);

//    gcontroller->control_torque(decition.torque());
//    gcontroller->control_brake(decition.brake());
//    gcontroller->control_engine(decition.engine());
//    gcontroller->control_grade(decition.grade());
//    gcontroller->control_mode(decition.mode());
//    gcontroller->control_handBrake(decition.handbrake());
//    gcontroller->control_speaker(decition.speak());
//    gcontroller->control_door(decition.door());
//    gcontroller->control_engine(decition.engine());
// //   gcontroller->control_light(decition.bri);
//    gcontroller->control_leftlight(decition.leftlamp());
//    gcontroller->control_rightlight(decition.rightlamp());
//    gcontroller->control_dangWei(decition.gear());

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

void UpdateChassis(const char *strdata, const unsigned int nSize, const unsigned int index, const QDateTime *dt, const char *strmemname)
{
    (void)index;
    (void)dt;
    (void)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.has_epsmode())&&(xchassis.epsmode() == 0))
    {
        gbChassisEPS = true;
    }


}


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

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

    if((oldtime - QDateTime::currentMSecsSinceEpoch())<-100)qDebug("dection time is %lld diff is %lld ",QDateTime::currentMSecsSinceEpoch(),oldtime - QDateTime::currentMSecsSinceEpoch());
    oldtime = QDateTime::currentMSecsSinceEpoch();
    gdecition.CopyFrom(xdecition);
    gnDecitionNum = gnDecitionNumMax;
    gbChassisEPS = false;

}

void ExecSend()
{
    iv::can::canmsg xmsg;
    iv::can::canraw xraw;
//    unsigned char * strp = (unsigned char *)&(ServiceControlStatus.command.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_ID);
    xraw.set_data(ServiceControlStatus.command.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);
    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.SerializePartialToArray(strser,ndatasize))
    {
        iv::modulecomm::ModuleSendMsg(gpacansend,strser,ndatasize);
    }
    else
    {
        std::cout<<"MainWindow::onTimer serialize error."<<std::endl;
    }
}

void initial()
{
    ServiceControlStatus.command.byte[0] = 0xEB;
    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(gnDecitionNum <= 0)
        {
            xdecition.CopyFrom(gdecition_def);
        }
        else
        {
            xdecition.CopyFrom(gdecition);
            gnDecitionNum--;
        }
        executeDecition(xdecition);
        if(gbChassisEPS == false) ExecSend();
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}

int main(int argc, char *argv[])
{
    RegisterIVBackTrace();
    showversion("controller_problue");
    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_mode(1);
    gdecition_def.set_grade(1);
    gdecition_def.set_gear(1);
//    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");

    gpacansend = iv::modulecomm::RegisterSend(gstrmemcansend.data(),10000,1);
    gpadecition = iv::modulecomm::RegisterRecv(gstrmemdecition.data(),ListenDeciton);
//    gpachassis = iv::modulecomm::RegisterRecv(gstrmemchassis.data(),UpdateChassis);

    std::thread xthread(sendthread);

    return a.exec();
}