modularization/src/ui/ui_ads_hmi_1px/ADCIntelligentVehicle.cpp

#include "ADCIntelligentVehicle.h"

#include "chassis.pb.h"
#include "xmlparam.h"

#define qtcout qDebug() << "[ " << __FILE__ << ":" << __LINE__<< " ]";

extern std::string gstrmemgps;
extern std::string gstrvin;
/**
     常量 	值 	描述
    QHostAddress::Null 	0 	空地址对象，相当于QHostAddress()。
    QHostAddress::LocalHost 	2 	IPv4本地主机地址，相当于QHostAddress(“127.0.0.1”)。
    QHostAddress::LocalHostIPv6 	3 	IPv6本地主机地址，相当于 QHostAddress(“::1”)。
    QHostAddress::Broadcast 	1 	Pv4广播地址，相当于QHostAddress(“255.255.255.255”)。
    QHostAddress::AnyIPv4 	6 	IPv4 any-address，相当于QHostAddress(“0.0.0.0”)。与该地址绑定的socket将只监听IPv4接口。
    QHostAddress::AnyIPv6 	5 	IPv6 any-address，相当于QHostAddress(“::”)。与该地址绑定的socket将只监听IPv4接口。
    QHostAddress::Any 	4 	双any-address栈，与该地址绑定的socket将侦听IPv4和IPv6接口。
*/

#define VIEW_CENTER viewport()->rect().center()
const double PI = 3.1415926535898;
extern iv::Ivlog * gIvlog;
/**
 * @brief MyView::MyView
 * @param parent
 */
MyView::MyView(QWidget *parent) :
    QGraphicsView(parent),
    beishu(1.00000)
{
    setDragMode(QGraphicsView::ScrollHandDrag);  //设置视图的拖拽模式 (光标变为指向手，然后拖动鼠标将滚动滚动条，此模式在交互和非交互模式下均有效)
}

/**
 * @brief MyView::mousePressEvent
 * @param event
 */
void MyView::mousePressEvent(QMouseEvent *event)
{
    bottonstatus = true;
    QGraphicsView::mousePressEvent(event);
}

/**
 * @brief MyView::mouseMoveEvent
 * @param event
 */
void MyView::mouseMoveEvent(QMouseEvent *event)
{
    QGraphicsView::mouseMoveEvent(event);
}


void MyView::keyPressEvent(QKeyEvent *event)
{
    //按键按下，key值放入容器，如果是长按触发的repeat就不判断
    if(!event->isAutoRepeat())
        mPressKeys.insert(event->key());

    if(event->key() == Qt::Key_J)
    {
        emit CtrlManual(true);
    }

    if(event->key() == Qt::Key_K)
    {
        emit CtrlManual(false);
    }
 //   qDebug("key count is %d",mPressKeys.count());
//    QDateTime dt;
//    qDebug("key press %ld",QDateTime::currentMSecsSinceEpoch());
//    qDebug("  key is %d",event->key());

}

void MyView::keyReleaseEvent(QKeyEvent *event)
{
    if(!event->isAutoRepeat())mPressKeys.remove(event->key());
    qDebug("key count is %d",mPressKeys.count());
//    QDateTime dt;
//    qDebug("key release %ld",QDateTime::currentMSecsSinceEpoch());
}

/**
 * @brief MyView::mouseReleaseEvent
 * @param event
 */
void MyView::mouseReleaseEvent(QMouseEvent *event)
{
    bottonstatus = false;
    QGraphicsView::mouseReleaseEvent(event);
}

ADCIntelligentVehicle * gAV;

//void ListenTraceMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//{

//    gAV->UpdateMap(strdata,nSize);

//}

//    void Listengpsimu(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//    {
//        gAV->UpdateGPSIMU(strdata,nSize);
//    }

//    void ListenRadar(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//    {

//        gAV->UpdateRADAR(strdata,nSize);

//    }

/**
 * @brief ListenCANState
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenCANState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    gIvlog->debug("ListenCanState");
    gAV->UpdateCANState(strdata,nSize);
}

//void ListenDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//{

//    gAV->UpdateDecition(strdata,nSize);

//}

//void ListenVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
//{

//    gAV->UpdateVehicleState(strdata,nSize);

//}

/**
 * @brief ListenOBS
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ListenOBS(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    //    qDebug("size is %d",nSize);
    std::shared_ptr<std::vector<iv::ObstacleBasic>> lidar_obs(new std::vector<iv::ObstacleBasic>);
    iv::ObstacleBasic * pdata = (iv::ObstacleBasic *)strdata;
    int nCount = nSize/sizeof(iv::ObstacleBasic);
    int i;
    for(i=0;i<nCount;i++)
    {
        iv::ObstacleBasic temp;
        memcpy(&temp,pdata,sizeof(iv::ObstacleBasic));
        lidar_obs->push_back(temp);
        pdata++;
    }
    gAV->UpdateOBS(lidar_obs);
    //  gw->UpdateOBS(lidar_obs);
}

//v2x使能状态请求
void ListenV2xStReq(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
//    gIvlog->info("hmi", "v2x enable request %d %d",nSize,sizeof(iv::v2x::v2xStReq));
//    if(nSize < sizeof(iv::v2x::v2xStReq))
//        return;

    iv::v2x::v2xStReq xv2xStReqMsg;
    if(!xv2xStReqMsg.ParseFromArray(strdata,nSize))
    {
        gIvlog->error("hmi", "iv::v2x::xv2xStReqMsg::ListenV2xStReq parse error");
        return;
    }
    if(xv2xStReqMsg.v2xstreq())
    {
        gAV->UpdateV2xStEn(gAV->mv2xStEn);
        gIvlog->info("hmi", "v2x enable request %d",gAV->mv2xStEn);
    }
}

/**
 * @brief ADCIntelligentVehicle::ADCIntelligentVehicle
 * @param parent
 */
ADCIntelligentVehicle::ADCIntelligentVehicle(QWidget *parent)
    : QMainWindow(parent),
      ui(new Ui::ADCIntelligentVehicle),
      m_translateButton(Qt::LeftButton),
      m_translateSpeed(1.0),
      m_zoomDelta(0.1),
      m_bMouseTranslate(false),
      m_scale(1.0),
      horizontalOffset(0),
      verticalOffset(0),
      scaleFactor(1),
      currentStepScaleFactor(1)
{
    ui->setupUi(this);
    gAV = this;
    gIvlog->info("ADCIntelligentVehicle Start Initialize");
    mTime.start();
/*
    QString strpath = QCoreApplication::applicationDirPath();
    strpath = strpath + "/ADS_decision.xml";
    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    ServiceCarStatus.mvehtype= xp.GetParam("vehType","ge3");

    if(ServiceCarStatus.mvehtype=="ge3"){
        ;
    }else if( ServiceCarStatus.mvehtype=="qingyuan"){
        ;
    }else if( ServiceCarStatus.mvehtype=="vv7"){
        ;
    }else if( ServiceCarStatus.mvehtype=="zhongche"){
        ;
    }
*/

    QString strpath = QCoreApplication::applicationDirPath();
    strpath = strpath + "/ui_ads_hmi.xml";
    iv::xmlparam::Xmlparam xp(strpath.toStdString());
    ServiceCarStatus.esr_y_offset = atof(xp.GetParam("esr_y_offset","2.5").data());
    ServiceCarStatus.lidar_y_offset = atof(xp.GetParam("lidar_y_offset","0").data());


    ui->listWidget->setIconSize(QSize(40,40));
    ui->stackedWidget->setCurrentIndex(0);

  //  QObject::connect(ui->listWidget,SIGNAL(currentRowChanged(int)),ui->stackedWidget,SLOT(on_listWidget_clicked()));//信号与槽
    QObject::connect(ui->listWidget,SIGNAL(currentRowChanged(int)),this,SLOT(on_listWidget_clicked()));//信号与槽

//    /********************************** 百度地图显示  ********************************/
//    mMapview = new QWebEngineView(ui->stackedWidget->widget(0));
////    qDebug((QDir::currentPath()).toLatin1().data());
//    mMapview->load(QUrl(QString("file:///%1/%2").arg(QApplication::applicationDirPath()).arg("BaiDuMap.html")));
//    mMapview->setGeometry(750,250,400,400);
//    /********************************************************************************/

    ui->listWidget->setCurrentRow(1);    //apollo_fu debug 20200409

    myview = new MyView(ui->stackedWidget->widget(1));
    myview->setObjectName(QStringLiteral("graphicsView"));
    myview->setGeometry(QRect(0, 100, 900, 900)); //从屏幕上（0，100）位置开始（即为最左上角的点），显示一个900*900的界面（宽900，高900）

#if 0
    myview_small = new MyView(ui->stackedWidget->widget(1));
    myview_small->setObjectName(QStringLiteral("graphicsView_small"));
    myview_small->setGeometry(QRect(1000,100,500,500));
    myview_small->setAlignment(Qt::AlignLeft | Qt::AlignTop);
#endif
//    myview_small->scale(0.7,0.7);
    //myview_small->centerOn(-200,-200);

    image = new QImage(900 * 2, 900 * 2, QImage::Format_RGB32);//画布的初始化大小设为600*500，使用32位颜色
    //QImage的32、24、8位图。 图像格式:QImage::Format_RGB32 ,QImage::Format_RGB888,QImage::Format_Indexed8。
    //QImage myImage2 = QImage(width, height, QImage::Format_…); 根据图像宽高来构造一幅图像，程序会自动根据图像格式对齐图像数据。
    image_small = new QImage(1800,1800,QImage::Format_RGB32);

    myview->setCacheMode(myview->CacheBackground);
#if 0
    myview_small->setCacheMode(myview_small->CacheBackground);
#endif
    painter = new QPainter(image);   //创建QPainter对象
    painter_small = new QPainter(image_small);   //创建QPainter对象
    scene = new QGraphicsScene;
    scene_small = new QGraphicsScene;
    timer = new QTimer(this);
    connect(timer, SIGNAL(timeout()), this, SLOT(timeoutslot()));//timeoutslot()为自定义槽
    timer->start(100);
    Lidar_obsread = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
    Lidar_read = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);

    mDataToUI.mHead.type = 1;
    mDataToUI.mInfo.accelerate = 0;
    mDataToUI.mInfo.brake = 0;
    mDataToUI.mInfo.gps_lat = 0;
    mDataToUI.mInfo.gps_lng = 0;
    mDataToUI.mInfo.is_mapLoad = 0;
    mDataToUI.mInfo.is_initSuccess = 0;
    mDataToUI.mInfo.is_run = 0;
    mDataToUI.mInfo.speed = 0;

    msockrecv.bind(QHostAddress::Any,9998);//绑定端口

    connect(&msockrecv,SIGNAL(readyRead()),this,SLOT(onRecvUDP()));  //接收来自PAD的UDP数据报   readyRead():socket读取缓冲区有数据时发送此信号，在此信号的槽函数里读取缓冲区的数据。

    ServiceCarStatus.speed = 0.0;
    mTimerState.setTimerType(Qt::PreciseTimer);    //设置计时器的精确程度    Qt::PreciseTimer值为0，含义：精确定时器试图保持毫秒精度
    connect(&mTimerState,SIGNAL(timeout()),this,SLOT(onStateTimer()));//广播发送UDP数据报
    mTimerState.start(100);   //定时100ms

    mTimerStateMap.setTimerType(Qt::PreciseTimer);    //
    connect(&mTimerStateMap,SIGNAL(timeout()),this,SLOT(onStateTimerMap()));//
    mTimerStateMap.start(500);   //定时500ms

    is_show_enable = true;

#ifdef USE_PAD_CTRL
    AutoStart();
#endif

    ModuleFun funplantrace = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaplantrace = iv::modulecomm::RegisterRecvPlus("plantrace",funplantrace);

    ModuleFun funplantrace_left = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_left,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaplantrace_left = iv::modulecomm::RegisterRecvPlus("obstraceleft",funplantrace_left);

    ModuleFun funplantrace_right = std::bind(&ADCIntelligentVehicle::UpdatePlanTrace_right,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaplantrace_right = iv::modulecomm::RegisterRecvPlus("obstraceright",funplantrace_right);

    ModuleFun funfusion = std::bind(&ADCIntelligentVehicle::UpdateFusion,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpafusion = iv::modulecomm::RegisterRecvPlus("li_ra_fusion",funfusion);


    ModuleFun funmap = std::bind(&ADCIntelligentVehicle::UpdateMap,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpa = iv::modulecomm::RegisterRecvPlus("tracemap",funmap);
    //   mpa = iv::modulecomm::RegisterRecv("tracemap",ListenTraceMap);

    ModuleFun fungpsimu =std::bind(&ADCIntelligentVehicle::UpdateGPSIMU,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpagpsimu = iv::modulecomm::RegisterRecvPlus(gstrmemgps.data(),fungpsimu);
    //    mpagpsimu = iv::modulecomm::RegisterRecv("gpsimu",Listengpsimu);

    ModuleFun funradar =std::bind(&ADCIntelligentVehicle::UpdateRADAR,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mparadar = iv::modulecomm::RegisterRecvPlus("radar",funradar);

    ModuleFun funchassis =std::bind(&ADCIntelligentVehicle::UpdateChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",funchassis);

    mpacanstate = iv::modulecomm::RegisterRecv("canstate",ListenCANState);
    gIvlog->debug("canState ptr: %x",mpacanstate);

    ModuleFun funbrainstate =std::bind(&ADCIntelligentVehicle::UpdateVehicleState,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
   // mpaVechicleState = iv::modulecomm::RegisterRecvPlus("vehiclestate",funbrainstate);
    mpaVechicleState = iv::modulecomm::RegisterRecvPlus("brainstate",funbrainstate);

    ModuleFun fundecition =std::bind(&ADCIntelligentVehicle::UpdateDecition,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);

//    mpaDecition = iv::modulecomm::RegisterRecvPlus("decition",fundecition);
    mpaDecition = iv::modulecomm::RegisterRecvPlus("deciton",fundecition);

    mpaLidar = iv::modulecomm::RegisterRecv("lidar_obs",ListenOBS);

    mpaHMI = iv::modulecomm::RegisterSend("hmi",10*sizeof(iv::hmi::HMIBasic),10);

    iv::modulecomm::RegisterRecv("v2xStReq", ListenV2xStReq);
    mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStEn",1000,1);

    mTimeState.start();

    mpManualCtrl = iv::modulecomm::RegisterSend("ivmanual",1000,1);
    mTimerManual = new QTimer(this);
    connect(mTimerManual,SIGNAL(timeout()),this,SLOT(onTimerManual()));
    connect(myview,SIGNAL(CtrlManual(bool)),this,SLOT(onCtrlManual(bool)));
    mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
//    timermanualctrl->start(10);

    //connect(ui->lineEdit_6, SIGNAL(textEdited(const QString &)), this, SLOT(savestabuyEditinfo(const QString &)));

    mpamapreq = iv::modulecomm::RegisterSend("mapreq",1000,1);
    shareMapReqMsg();

    ModuleFun funvbox = std::bind(&ADCIntelligentVehicle::UpdateVbox,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
    mpaplantrace = iv::modulecomm::RegisterRecvPlus("vbox",funvbox);
#if 1 //红绿灯
    ui->lcdTurn->setDigitCount(3); //设置显示几个数字
    ui->lcdTurn->setMode(QLCDNumber::Dec);
    ui->lcdTurn->setSegmentStyle(QLCDNumber::Flat);//设置数字字体：Outline,轮廓突出，颜色为背景色；Filled，内部填充型，颜色为黑色；Flat,平面型
    // 设置数字颜色时要注意：  函数setSegmentStyle(QLCDNumber::Flat)中选择Flat属性，否则在setColor()设置数字颜色时不生效
    // 设置背景色直接用setStyleSheet()函数设定就可以了。
    //调色板
    QPalette lcdpat = ui->lcdTurn->palette();
    /*设置颜色，整体背景颜色 颜色蓝色,此函数的第一个参数可以设置多种。如文本、按钮按钮文字、多种*/
    lcdpat.setColor(QPalette::Normal,QPalette::WindowText,Qt::red);
    //设置当前窗口的调色板
    ui->lcdTurn->setPalette(lcdpat);
    //设置背景色
    //ui->lcdNumber->setStyleSheet("background-color: black");
    //ui->lcdNumber->display(QDateTime::currentDateTime().toString("yyyy-MM-dd hh:mm:ss"));
    ui->lcdTurn->display(0);

    ui->lcdLeft->setDigitCount(3);
    ui->lcdLeft->setMode(QLCDNumber::Dec);
    ui->lcdLeft->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdLeft->setPalette(lcdpat);
    //ui->lcdLeft->setStyleSheet("background-color: black");
    ui->lcdLeft->display(0);

    ui->lcdStraight->setDigitCount(3);
    ui->lcdStraight->setMode(QLCDNumber::Dec);
    ui->lcdStraight->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdStraight->setPalette(lcdpat);
    //ui->lcdStraight->setStyleSheet("background-color: black");
    ui->lcdStraight->display(0);

    ui->lcdRight->setDigitCount(3);
    ui->lcdRight->setMode(QLCDNumber::Dec);
    ui->lcdRight->setSegmentStyle(QLCDNumber::Flat);
    ui->lcdRight->setPalette(lcdpat);
    //ui->lcdRight->setStyleSheet("background-color: black");
    ui->lcdRight->display(0);
    // 新建qimage
    // QImage img,img2,img3,img4;
    // 加载图片
    img.load(":/light_image/diaotou-black.png");
    img = img.scaled(66,66,Qt::IgnoreAspectRatio);
    ui->label_38->setPixmap(QPixmap::fromImage(img));
    img2.load(":/light_image/zuoguai-black.png");
    img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
    ui->label_33->setPixmap(QPixmap::fromImage(img2));
    img3.load(":/light_image/zhixing-black.png");
    img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
    ui->label_35->setPixmap(QPixmap::fromImage(img3));
    img4.load(":/light_image/youguai-black.png");
    img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
    ui->label_32->setPixmap(QPixmap::fromImage(img4));

    mTimerState1.setTimerType(Qt::PreciseTimer);    //设置计时器的精确程度    Qt::PreciseTimer值为0，含义：精确定时器试图保持毫秒精度
    connect(&mTimerState1,SIGNAL(timeout()),this,SLOT(onStateTimerTraffic()));//广播发送UDP数据报
    mTimerState1.start(100);   //定时100ms

    #endif

    ui->lineEdit_vin->setText(QString::fromStdString(gstrvin));
}

/**
 * @brief ADCIntelligentVehicle::savestabuyEditinfo
 * @param txt
 */
void ADCIntelligentVehicle::savestabuyEditinfo(const QString &txt)
{
    //ServiceControlStatus.set_accelerate((float)txt.toDouble());
}



/**
 * @brief ADCIntelligentVehicle::timeoutslot
 */
void ADCIntelligentVehicle::timeoutslot()
{
    update();

    /////////////////////////////////
    //                             //
    //      实时状态信息显示         //
    ////////////////////////////////
    QString is_ok;
    ui->lineEdit_RTK->setText(QString::number(ServiceCarStatus.location->rtk_status));//rtk状态
    if(ServiceCarStatus.location->rtk_status==6)
    {
        ui->button_RTK_st->setStyleSheet("background-color: green");
    }
    else
    {
        ui->button_RTK_st->setStyleSheet("background-color: red");
    }

    ui->lineEdit_INS->setText(QString::number(ServiceCarStatus.location->ins_status));//ins状态
    if(ServiceCarStatus.location->ins_status==4)
        ui->button_INS_st->setStyleSheet("background-color: green");
    else
        ui->button_INS_st->setStyleSheet("background-color: red");

    is_ok = (ServiceCarStatus.mLidarS>0)?QStringLiteral("ok"):QStringLiteral("error");
    ui->lineEdit_lidar->setText(is_ok);//激光雷达状态
    if(ServiceCarStatus.mLidarS>0)
    {
        ui->button_lidar_st->setStyleSheet("background-color: green");
    }
    else
    {
        ui->button_lidar_st->setStyleSheet("background-color:red");
    }

    ui->lineEdit_dec_period->setText(QString::number(mdecition_period)+QStringLiteral("ms"));//决策周期
    if(mdecition_period>0)
    {
        ui->button_decition_st->setStyleSheet("background-color: green");
    }

    if((mTimeState.elapsed()-mnTimeUpdateGPS) > 1000)
    {
        ui->button_RTK_st->setStyleSheet("background-color: red");
        ui->button_INS_st->setStyleSheet("background-color: red");
    }

    if((mTimeState.elapsed() - mnTimeUpdateDecition)>1000)
    {
        ui->button_decition_st->setStyleSheet("background-color:red");
        ui->label_decition->setText("No decition");
        mbBrainRunning = false;
    }
    else
    {
        QString strdec = "Acc:";
        strdec = strdec + QString::number(ServiceCarStatus.mfAcc,'f',1);
        //   strdec = strdec + " Brake:";
        //   strdec = strdec + QString::number(ServiceCarStatus.mfBrake,'f',1);
        strdec = strdec + " EPS:";
        strdec = strdec + QString::number(ServiceCarStatus.mfWheel,'f',1);
        ui->label_decition->setText(strdec);
    }

    if(mbManual)
    {
        char strmanual[256];
        snprintf(strmanual,256,"Manual %d %d %d",mnCtrlValue[0],mnCtrlValue[1],mnCtrlValue[2]);
        ui->label_decition->setText(strmanual);
    }

    /////////////////////////////////
    //                             //
    //      实时显示障碍物状态        //
    /////////////////////////////////
    //    ServiceCarStatus.mObs = 10;
    //    ServiceCarStatus.mLidarObs = 9.123;
    //    ServiceCarStatus.mRadarObs = 9.222;
    //    ServiceCarStatus.mfttc = 0.5;
    //    ServiceCarStatus.mfBrake = 20;
    if(ServiceCarStatus.mObs > 0)
    {
        ui->label_13->setText("Obs:"+QString::number(ServiceCarStatus.mObs,'f',1)+" Lidar:"
                              +QString::number(ServiceCarStatus.mLidarObs,'f',1) + " Radar:"
                              +QString::number(ServiceCarStatus.mRadarObs,'f',1));
    }
    else
    {
        ui->label_13->setText(QString("前方没有障碍物"));
    }

    ui->label_26->setText("ttc is "+QString::number(ServiceCarStatus.mfttc,'f',1) + " brake is " + QString::number(ServiceCarStatus.mfBrake,'f',2));  //add 20200518

    QString strstate = "Map:";
    if(mDataToUI.mInfo.is_mapLoad == 1)
    {
        strstate = strstate+"Yes  ";
    }
    else
    {
        strstate = strstate+"No ";
    }
    strstate = strstate+" CANCARD:";
    if(mDataToUI.mInfo.is_initSuccess == 1)
    {
        strstate = strstate+"OK ";
    }
    else
    {
        strstate = strstate+"Fail ";
    }
    strstate = strstate + " Run State:";
    if(mbBrainRunning)
    {
        strstate = strstate+"RUN";
    }
    else
    {
        strstate = strstate+"STOP";
    }

    ui->label_State->setText(strstate);

    ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed,'f',1));


    //    QString is_ok;
    //    is_ok = (ServiceLidar.did_lidar_ok() == true)?QStringLiteral("ok"):QStringLiteral("error");
    //    ui->lineEdit_4->setText(is_ok);
    //can state
    mDataToUI.mInfo.is_initSuccess = 0;
}

/**
 * @brief ADCIntelligentVehicle::on_pb_load_navigation_data_clicked
 */
//void ADCIntelligentVehicle::on_pb_load_navigation_data_clicked()  //加载地图
//{

//}

/**
 * @brief ADCIntelligentVehicle::onRecvUDP
 */
void ADCIntelligentVehicle::onRecvUDP()
{
    while (msockrecv.hasPendingDatagrams())  //hasPendingDatagrams()表示是否有待读取的传入数据报
    {
        qDebug("recv cmd");
        QByteArray ba;
        ba.resize(msockrecv.pendingDatagramSize());//pendingDatagramSize()返回待读取数据报的字节数
        msockrecv.readDatagram(ba.data(), ba.size()); //readDatagram()函数用于读取数据报内容
        int * p;
        iv::hmi::HMIBasic hmi;
        p = (int *)ba.data();
        //       qDebug("cmd is ",*p);
        //      std::cout<<"data len is "<<ba.size()<<" value is "<<*p<<std::endl;
        if(*p == 1)
        {
            std::cout<<ServiceCarStatus.mRunTime.elapsed()<<":"<<"recv start."<<std::endl;
            ServiceCarStatus.mbRunPause = false;
            hmi.mbBocheMode = false;
            hmi.mbPause = false;
            ShareHMIMsg(hmi);

        }
        if(*p == 0)
        {
            ServiceCarStatus.mbRunPause = true;
            hmi.mbBocheMode = false;
            hmi.mbPause = true;
            ShareHMIMsg(hmi);
        }
        if(*p == 2)
        {
            ServiceCarStatus.bocheMode = true;
            hmi.mbBocheMode = true;
            hmi.mbPause = false;
            ShareHMIMsg(hmi);
        }

        if(*p == 3)
        {
            hmi.mbbusmode = true;
            hmi.mbPause = ServiceCarStatus.mbRunPause;
            ShareHMIMsg(hmi);
        }
        if(*p == 4)
        {
            hmi.mbbusmode = false;
            hmi.mbPause = ServiceCarStatus.mbRunPause;
            ShareHMIMsg(hmi);
        }
    }
}

/**
 * @brief ADCIntelligentVehicle::onStateTimer
 */
void ADCIntelligentVehicle::onStateTimer()
{

    if(ServiceCarStatus.mbBrainCtring)
        mDataToUI.mInfo.is_run = 1;
    else
        mDataToUI.mInfo.is_run = 0;

    if(ServiceCarStatus.mLidarS<0)
        mDataToUI.mInfo.lidarStatus = 0;
    else
        mDataToUI.mInfo.lidarStatus = 1;

    if(ServiceCarStatus.mRadarS<0)
        mDataToUI.mInfo.radarStatus = 0;
    else
        mDataToUI.mInfo.radarStatus = 1;

    ServiceCarStatus.mLidarS--;  //如果从10减到0，表示没有接收到数据。
    ServiceCarStatus.mRadarS--;

    mDataToUI.mInfo.RTKstatus = ServiceCarStatus.mRTKStatus;
    ServiceCarStatus.mRTKStatus = 0;

    mDataToUI.mInfo.radarobs = ServiceCarStatus.mRadarObs;
    mDataToUI.mInfo.lidarobs = ServiceCarStatus.mLidarObs;
    mDataToUI.mInfo.obs = ServiceCarStatus.mObs;

    mDataToUI.mInfo.gps_lat = ServiceCarStatus.location->gps_lat;
    mDataToUI.mInfo.gps_lng = ServiceCarStatus.location->gps_lng;
    mDataToUI.mInfo.accelerate = ServiceCarStatus.mfAcc;
    mDataToUI.mInfo.speed = ServiceCarStatus.speed;
    mDataToUI.mInfo.brake = ServiceCarStatus.mfBrake;
    mDataToUI.mInfo.swerve = ServiceCarStatus.mfWheel;
    mDataToUI.mInfo.boche_status = ServiceCarStatus.bocheEnable;

    if(fabs(mDataToUI.mInfo.gps_lat)<1 )
    {
        //       qDebug("gps lat is error.");
        //    std::cout<<"no gps."<<std::endl;
    }
    //  std::cout<<"send data."<<std::endl;
    //    qDebug("speed is %d",mDataToUI.mInfo.speed);
    //   mDataToUI.mInfo.is_initSuccess = 1;
    //   mDataToUI.mInfo.speed = 36.1;
    msocksend.writeDatagram((char *)&mDataToUI,sizeof(DataToUI),QHostAddress::Broadcast,9999);   //QHostAddress::Broadcast 	值为1 	Pv4广播地址，相当于QHostAddress(“255.255.255.255”)。
    //   _udp->writeDatagram(str.toUtf8(), QHostAddress::Broadcast, 10002);



    // multicast, 224.0.0.1~224.0.0.255 is multicast address of LAN

    //   _udp->writeDatagram(str.toUtf8(), QHostAddress("224.0.0.131"), 10002);

}
void ADCIntelligentVehicle::onStateTimerMap()
{
    /*******************************更新百度地图******************************/

    double flat = ServiceCarStatus.location->gps_lat;
    double flon = ServiceCarStatus.location->gps_lng;
    double fang = ServiceCarStatus.location->ins_heading_angle;

    char strscript[256];
    snprintf(strscript,255,"theLocation(%11.7f,%11.7f,%11.7f);",flon,flat,fang);
    /************************************************************************/
}

/**
 * @brief ADCIntelligentVehicle::AutoStart
 */
void ADCIntelligentVehicle::AutoStart()
{

    //    is_brain_running_flag_ = true;
    //    mDataToUI.mInfo.is_initSuccess = 1;
    //    std::cout<<ServiceCarStatus.mRunTime.elapsed()<<"ms:"<<"Start Run SuccessFully."<<std::endl;

}

void ADCIntelligentVehicle::onStateTimerTraffic()
{
    ui->lcdTurn->display((int)ServiceCarStatus.time_turn);
    ui->lcdLeft->display((int)ServiceCarStatus.time_left);
    ui->lcdStraight->display((int)ServiceCarStatus.time_straight);
    ui->lcdRight->display((int)ServiceCarStatus.time_right);
    switch (ServiceCarStatus.st_turn) {
    case 0:
        img.load(":/light_image/diaotou-black.png");
        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_38->setPixmap(QPixmap::fromImage(img));
        break;
    case 1:
        img.load(":/light_image/diaotou-green.png");
        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_38->setPixmap(QPixmap::fromImage(img));
        break;
    case 2:
        img.load(":/light_image/diaotou-red.png");
        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_38->setPixmap(QPixmap::fromImage(img));
        break;
    case 3:
        img.load(":/light_image/diaotou-yellow.png");
        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_38->setPixmap(QPixmap::fromImage(img));
        break;
    default:
        img.load(":/light_image/diaotou-black.png");
        img = img.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_38->setPixmap(QPixmap::fromImage(img));
        break;
    }
    switch (ServiceCarStatus.st_left) {
    case 0:
        img2.load(":/light_image/zuoguai-black.png");
        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_33->setPixmap(QPixmap::fromImage(img2));
        break;
    case 1:
        img2.load(":/light_image/zuoguai-green.png");
        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_33->setPixmap(QPixmap::fromImage(img2));
        break;
    case 2:
        img2.load(":/light_image/zuoguai-red.png");
        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_33->setPixmap(QPixmap::fromImage(img2));
        break;
    case 3:
        img2.load(":/light_image/zuoguai-yellow.png");
        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_33->setPixmap(QPixmap::fromImage(img2));
        break;
    default:
        img2.load(":/light_image/zuoguai-black.png");
        img2 = img2.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_33->setPixmap(QPixmap::fromImage(img2));
        break;
    }
    switch (ServiceCarStatus.st_straight) {
    case 0:
        img3.load(":/light_image/zhixing-black.png");
        img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_35->setPixmap(QPixmap::fromImage(img3));
        break;
    case 1:
        img3.load(":/light_image/zhixing-green.png");
        img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_35->setPixmap(QPixmap::fromImage(img3));
        break;
    case 2:
        img3.load(":/light_image/zhixing-red.png");
        img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_35->setPixmap(QPixmap::fromImage(img3));
        break;
    case 3:
        img3.load(":/light_image/zhixing-yellow.png");
        img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_35->setPixmap(QPixmap::fromImage(img3));
        break;
    default:
        img3.load(":/light_image/zhixing-black.png");
        img3 = img3.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_35->setPixmap(QPixmap::fromImage(img3));
        break;
    }
    switch (ServiceCarStatus.st_right) {
    case 0:
        img4.load(":/light_image/youguai-black.png");
        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_32->setPixmap(QPixmap::fromImage(img4));
        break;
    case 1:
        img4.load(":/light_image/youguai-green.png");
        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_32->setPixmap(QPixmap::fromImage(img4));
        break;
    case 2:
        img4.load(":/light_image/youguai-red.png");
        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_32->setPixmap(QPixmap::fromImage(img4));
        break;
    case 3:
        img4.load(":/light_image/youguai-yellow.png");
        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_32->setPixmap(QPixmap::fromImage(img4));
        break;
    default:
        img4.load(":/light_image/youguai-black.png");
        img4 = img4.scaled(66,66,Qt::IgnoreAspectRatio);
        ui->label_32->setPixmap(QPixmap::fromImage(img4));
        break;
    }
}

/**
 * @brief ADCIntelligentVehicle::on_pb_open_can_card_clicked
 */
//void ADCIntelligentVehicle::on_pb_open_can_card_clicked() {

//}

/**
 * @brief ADCIntelligentVehicle::on_timer_car_status_time_out
 */
//void ADCIntelligentVehicle::on_timer_car_status_time_out() {
//    ui->lb_current_speed_big->setText(QString::number(ServiceCarStatus.speed));
//    //ui->sb_accelerate_percent->setValue(ServiceControlStatus.accelerate_percent);
//    //ui->sb_wheel_percent->setValue(ServiceControlStatus.wheel_angle);

//}

/**
 * @brief ADCIntelligentVehicle::keyPressEvent  加/减键进行缩放
 * @param event
 */
void ADCIntelligentVehicle::keyPressEvent(QKeyEvent *event)
{
    switch (event->key()) {
    case Qt::Key_Plus:  // 放大
        myview->zoomIn();
        break;
    case Qt::Key_Minus:  // 缩小
        myview->zoomOut();
        break;
    default:
        QMainWindow::keyPressEvent(event);
    }
}

/**
 * @brief ADCIntelligentVehicle::closeEvent
 * @param event
 */
void ADCIntelligentVehicle::closeEvent(QCloseEvent *event)
{
    QMessageBox::StandardButton button;
    button=QMessageBox::question(this,tr("退出程序"),QString(tr("确认退出程序")),QMessageBox::Yes|QMessageBox::No);
    if(button==QMessageBox::No)
    {
        event->ignore(); // 忽略退出信号，程序继续进行
    }
    else if(button==QMessageBox::Yes)
    {
        event->accept(); // 接受退出信号，程序退出
    }
}

/**
 * @brief MyView::wheelEvent  放大/缩小
 * @param event
 */
void MyView::wheelEvent(QWheelEvent *event)
{
    // 滚轮的滚动量
    QPoint scrollAmount = event->angleDelta();
    // 正值表示滚轮远离使用者（放大），负值表示朝向使用者（缩小）
    scrollAmount.y() > 0 ? zoomIn() : zoomOut();
}

/**
 * @brief MyView::zoomIn 放大
 */
void MyView::zoomIn()
{
    scale(1.1, 1.1);
    beishu *= 1.1;
    centerOn(450, 700 - (200 / beishu));
}

/**
 * @brief MyView::zoomOut  缩小
 */
void MyView::zoomOut()
{
    scale(1 / 1.1, 1 / 1.1);
    beishu /= 1.1;
    centerOn(450, 700 - (200 / beishu));
}

/**
 * @brief ADCIntelligentVehicle::paintEvent  刷新
 */
void ADCIntelligentVehicle::paintEvent(QPaintEvent *)
{
    if (is_show_enable)
    {
        //       qDebug("enter paint. time1 is %d",mTime.elapsed());

        if(fabs(mTime.elapsed() - mnTimeLastUpdatePaint)<100)
        {
            return;
        }
        mnTimeLastUpdatePaint = mTime.elapsed();
        //       qDebug("enter paint. time is %d",mTime.elapsed());
        painter->begin(image);
//        painter_small->begin(image_small);

       // image->fill(QColor(60, 60, 60));//对画布进行填充
       image->fill(QColor(220, 220, 220));//对画布进行填充
       image_small->fill(QColor(220,220,220));
        std::vector<iv::GPSData> navigation_data;
        mMutexNavi.lock();
        navigation_data = m_navigation_data;
        mMutexNavi.unlock();

        std::vector<iv::Point2D> myplan,myplan_left,myplan_right;
        mMutexPlan.lock();
        myplan = m_plan;
        mMutexPlan.unlock();

        mMutexPlan_left.lock();
        myplan_left = m_plan_left;
        mMutexPlan_left.unlock();

        mMutexPlan_right.lock();
        myplan_right = m_plan_right;
        mMutexPlan_right.unlock();
//        std::cout<<"plan size is "<<myplan.size()<<std::endl;



        //        std::vector<iv::GPSData> navigation_data = brain->navigation_data;
        painter->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点
        painter_small->setRenderHint(QPainter::Antialiasing, true);//设置反锯齿模式，好看一点


        int pointx = 450, pointy = 700;//确定坐标轴起点坐标，这里定义(35,280)
//        int pointx_small = 450, pointy_small = 700;
        //        double x0[22000], y0[22000], lng[22000], x1[22000], y1[22000], x2[22000], y2[22000];

                double * x0, * y0, * lng, * x1, * y1, * x2, * y2;
                std::shared_ptr<double> ptrx0,ptry0,ptrlng,ptrx1,ptry1,ptrx2,ptry2;
                int nmapsize = navigation_data.size();
                x0 = new double[nmapsize];
                y0 = new double[nmapsize];
                lng = new double[nmapsize];
                x1 = new double[nmapsize];
                y1 = new double[nmapsize];
                x2 = new double[nmapsize];
                y2 = new double[nmapsize];

                ptrx0.reset(x0);
                ptry0.reset(y0);
                ptrlng.reset(lng);
                ptrx1.reset(x1);
                ptry1.reset(y1);
                ptrx2.reset(x2);
                ptry2.reset(y2);
        double xx = 0, yy = 0;
        double sumx = 0, sumy = 0;//, ave_x = 0, ave_y = 0;
        int sizeN = navigation_data.size();
        //int max_x_pos = 0, min_x_pos = 0, max_y_pos = 0, min_y_pos = 0;
        int x_max = 0, y_max = 0;//数组里的最大值
        int x_min = 0x3f3f3f3f, y_min = 0x3f3f3f3f;//inf为 #define inf 0x3f3f3f3f
        double k1, k2;
        QPen pen, penPoint;

        //先绘制车位置点及框图
        static const QPointF points1[2] = { QPointF(300, 700), QPointF(600, 700) };
        static const QPointF points2[2] = { QPointF(450, 0), QPointF(450, 900) };
//        static const QPointF points1_small[2] = { QPointF(300, 700), QPointF(600, 700) };
//        static const QPointF points2_small[2] = { QPointF(450, 0), QPointF(450, 900) };

        penPoint.setColor(Qt::red);
        penPoint.setWidth(2);
        painter->setPen(penPoint);
//        painter_small->setPen(penPoint);
        painter->drawPoint(pointx, pointy);
//        painter_small->drawPoint(pointx_small,pointy_small);
        painter->drawPolyline(points1, 2);
        painter->drawPolyline(points2, 2);
//        painter_small->drawPolyline(points1_small, 2);
//        painter_small->drawPolyline(points2_small, 2);

        //路径点的预处理
        for (int i = 0; i < sizeN; i++)
        {
            x0[i] = navigation_data[i]->gps_x;
            y0[i] = navigation_data[i]->gps_y;
            lng[i] = navigation_data[i]->ins_heading_angle;
            sumx = sumx + navigation_data[i]->gps_x;
            sumy = sumy + navigation_data[i]->gps_y;
            if (x0[i] > x_max) {
                x_max = x0[i];
                //max_x_pos = i;
            }
            if (x0[i] < x_min) {
                x_min = x0[i];
                //min_x_pos = i;
            }

            if (y0[i] > y_max) {
                y_max = y0[i];
                //max_y_pos = i;
            }
            if (y0[i] < y_min) {
                y_min = y0[i];
                //min_y_pos = i;
            }
        }
        //ave_x = sumx / sizeN;
        //ave_y = sumy / sizeN;

        //获取到实时 GPS信息，并做路径点的显示更新
        if (ServiceCarStatus.location->gps_x == 0)
        {
            painter->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
//            painter_small->drawText(rect(), Qt::AlignLeft, QString::fromLocal8Bit("等待车辆实时GPS位置信息"));
        }
        else
        {
            x0[0] = ServiceCarStatus.location->gps_x;
            y0[0] = ServiceCarStatus.location->gps_y;
            //          std::cout<<"x = "<<x0[0]<<" y= "<<y0[0]<<std::endl;
            lng[0] = ServiceCarStatus.location->ins_heading_angle;
        }
        //根据标定原点的选取，对路径点进行转化
        for (int i = 1; i < sizeN; i++)
        {
            //         if(i == 1)std::cout<<" x1 = "<<x0[i]<<std::endl;
            x0[i] = x0[i] - x0[0];
            y0[i] = y0[i] - y0[0];
            xx = x0[i];
            yy = y0[i];
            x0[i] = xx * cos(lng[0] * PI / 180) - yy * sin(lng[0] * PI / 180);
            y0[i] = xx * sin(lng[0] * PI / 180) + yy * cos(lng[0] * PI / 180);
            k1 = sin((90 + (lng[i] - lng[0])) * PI / 180);
            k2 = cos((90 + (lng[i] - lng[0])) * PI / 180);
            #if 0
            x1[i] = x0[i] + k1 * 5;
            y1[i] = y0[i] + k2 * 5;
            x2[i] = x0[i] - k1 * 5;
            y2[i] = y0[i] - k2 * 5;
            #else
            x1[i] = x0[i] + k1 * 1.75;
            y1[i] = y0[i] + k2 * 1.75;
            x2[i] = x0[i] - k1 * 1.75;
            y2[i] = y0[i] - k2 * 1.75;
            #endif
        }

//        double kx_small = (double)(1200) / (abs(y_max - y_min));//x轴的系数
//        double ky_small = (double)(1200) / (abs(y_max - y_min));//y方向的比例系数

        double kx = 10;
        double ky = 10;

        //距离车正前方10m处画一条线
        static const QPointF points3[2] = { QPointF(0, 700 - 10 * ky), QPointF(900, 700 - 10 * ky) };
        painter->drawPolyline(points3, 2);

        //绘制路径点
        penPoint.setColor(Qt::black);
        penPoint.setWidth(1);

        for (int i = 1; i < sizeN - 1; i++)
        {
            painter->setPen(penPoint);//蓝色的笔，用于标记各个点
//            painter_small->setPen(penPoint);//蓝色的笔，用于标记各个点
            painter->drawPoint(pointx + x0[i] * kx, pointy - y0[i] * ky);
//            painter_small->drawPoint(pointx_small + x0[i] * kx_small, pointy_small - y0[i] * ky_small);
            painter->drawPoint(pointx + x1[i] * kx, pointy - y1[i] * ky);
//            painter_small->drawPoint(pointx_small + x1[i] * kx_small, pointy_small - y1[i] * ky_small);
            painter->drawPoint(pointx + x2[i] * kx, pointy - y2[i] * ky);
//            painter_small->drawPoint(pointx_small + x2[i] * kx_small, pointy_small - y2[i] * ky_small);
        }
        painter->drawPoint(pointx + x0[sizeN - 1] * kx, pointy - y0[sizeN - 1] * ky);//绘制最后一个点
//        painter_small->drawPoint(pointx_small + x0[sizeN - 1] * kx_small, pointy_small - y0[sizeN - 1] * ky_small);//绘制最后一个点

        penPoint.setColor(Qt::red);
        penPoint.setWidth(2);

        painter->drawPoint(pointx + x0[0] * kx, pointy - y0[0] * ky);
//        painter_small->drawPoint(pointx_small + x0[0] * kx_small, pointy_small - y0[0] * ky_small);

#if 1
        // draw plan trace
        penPoint.setColor(QColor(187, 255, 255, 100));
        penPoint.setWidth(2);
        painter->setPen(penPoint);
        QPointF tracePoints[myplan.size()];
        if(myplan.size()>2)
        {
            for(int i=0;i<(myplan.size()-1);i++)
            {
                tracePoints[i].setX((float)(450+myplan[i].x*kx));
                tracePoints[i].setY((float)(700-myplan[i].y*ky));
                painter->drawLine((float)(450+myplan[i].x*kx),(float)(700-myplan[i].y*ky),
                                  (float)(450+myplan[i+1].x*kx),(float)(700-myplan[i+1].y*ky));
            }
        }
#endif
#if 1
//        penPoint.setColor(Qt::gray);
        penPoint.setColor(QColor(187, 255, 255, 100));
        penPoint.setWidth(2);
        painter->setPen(penPoint);
        QPointF tracePoints_left[myplan_left.size()];
        if(myplan_left.size()>2)
        {
            for(int i=0;i<(myplan_left.size()-1);i++)
            {
                tracePoints_left[i].setX((float)(450+myplan_left[i].x*kx));
                tracePoints_left[i].setY((float)(700-myplan_left[i].y*ky));
                painter->drawLine((float)(450+myplan_left[i].x*kx),(float)(700-myplan_left[i].y*ky),
                                  (float)(450+myplan_left[i+1].x*kx),(float)(700-myplan_left[i+1].y*ky));
            }
        }

//        penPoint.setColor(Qt::gray);
//        penPoint.setWidth(2);
//        painter->setPen(penPoint);
        QPointF tracePoints_right[myplan_right.size()];
        if(myplan_right.size()>2)
        {
            for(int i=0;i<(myplan_right.size()-1);i++)
            {
                tracePoints_right[i].setX((float)(450+myplan_right[i].x*kx));
                tracePoints_right[i].setY((float)(700-myplan_right[i].y*ky));
                painter->drawLine((float)(450+myplan_right[i].x*kx),(float)(700-myplan_right[i].y*ky),
                                  (float)(450+myplan_right[i+1].x*kx),(float)(700-myplan_right[i+1].y*ky));
            }
        }
#endif
 //       painter->drawPolyline(tracePoints,myplan.size());
        // draw plan trace end
/////////////////////////////////////apollo add car icon  20200409
        QPixmap pix;
        //pix.load("car.png");
        pix.load(":/ADCIntelligentVehicle/car1.png");
        painter->drawPixmap(435,667,30,67,pix);
//        painter_small->drawPixmap(442,683,16,34,pix);
///////////////////////////////////////////////////////////////////


        penPoint.setColor(Qt::blue);
        penPoint.setWidth(3);
        painter->setPen(penPoint);

        QFont font;
        font.setFamily("Microsoft YaHei");
        font.setPointSize(50);
        font.setItalic(true);
        painter->setFont(font);

        //////////////////////////////////////
        //                                  //
        //	显示毫米波雷达和激光雷达的信息       //
        //									//
        //////////////////////////////////////

        if(ui->checkBox_lidar_show->isChecked())
        {
            ServiceLidar.copylidarto(Lidar_read);		//激光雷达障碍物
            ServiceLidar.copylidarobsto(Lidar_obsread);
            painter->setPen(QColor(255, 0, 0));
            for (unsigned int x = 0; x < Lidar_read->size(); x++)
            {
                //painter->drawPoint(((*Lidar_read)[x].nomal_x) * 10 + 450, -(*Lidar_read)[x].nomal_y * 10 + 700);
            }
            painter->setPen(QColor(255,0,0));
            for (unsigned int x = 0; x < Lidar_obsread->size(); x++)
            {
                painter->drawPoint(((*Lidar_obsread)[x].nomal_x) * 10 + 450, -((*Lidar_obsread)[x].nomal_y + ServiceCarStatus.lidar_y_offset) * 10 + 700);
            }
        }


        //////////////////////////////////////
        //                                  //
        //	          显示融合的信息          //
        //									//
        //////////////////////////////////////

        if(ui->checkBox_fusion_show->isChecked())
        {
            painter->setPen(QColor(255,0,0));
            iv::fusion::fusionobjectarray xfusionarray;
            if(mbfusionUpdate)
            {
                mMutexFusion.lock();
                xfusionarray.CopyFrom(mfusionarray);
                mMutexFusion.unlock();
                for(int a = 0; a < xfusionarray.obj_size(); a++)
                {
                    for(int b = 0; b < xfusionarray.obj(a).nomal_centroid_size(); b++)
                    {
                        painter->drawPoint((xfusionarray.obj(a).nomal_centroid(b).nomal_x())*10 + 450, -(xfusionarray.obj(a).nomal_centroid(b).nomal_y() + ServiceCarStatus.lidar_y_offset)*10 + 700);
                    }
                }
            }
         }




        painter->end();
//        painter_small->end();
        scene->clear();
//        scene_small->clear();
        scene->addPixmap(QPixmap::fromImage(*image));
//        scene_small->addPixmap(QPixmap::fromImage(*image_small));
        myview->setScene(scene);
//        myview_small->setScene(scene_small);
        myview->show();
//        myview_small->show();
        navigation_data.clear();
    }
}

/**
 * @brief ADCIntelligentVehicle::on_listWidget_clicked
 */
void ADCIntelligentVehicle::on_listWidget_clicked()
{
    int currentRow=ui->listWidget->currentRow(); //currentRow()获取当前行号

    if(currentRow==0) //点击第一切换到第一页
    {
        ui->stackedWidget->setCurrentIndex(0);//为堆栈窗体切换到当前索引
    }

    else if(currentRow==1)
    {
        ui->stackedWidget->setCurrentIndex(1);
    }
    else if(currentRow==2)
    {
        ui->stackedWidget->setCurrentIndex(2);
    }

    else
    {
        ui->stackedWidget->setCurrentIndex(0);
    }
}

/**
 * @brief ADCIntelligentVehicle::UpdatePlanTrace
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ADCIntelligentVehicle::UpdatePlanTrace(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    mMutexPlan.lock();
    m_plan.clear();
    int nplansize = nSize/sizeof(iv::Point2D);
    int npsize = sizeof(iv::Point2D);
    int i;
    for(i=0;i<nplansize;i++)
    {
        iv::Point2D x;
        memcpy(&x,strdata + i*npsize,npsize);
        m_plan.push_back(x);
    }
    mMutexPlan.unlock();
}

void ADCIntelligentVehicle::UpdatePlanTrace_left(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    mMutexPlan_left.lock();
    m_plan_left.clear();
    int nplansize = nSize/sizeof(iv::Point2D);
    if(nplansize < 1)
        std::cout<<"UpdatePlanTrace_left size is 0................. "<<std::endl;
//        return;
    int npsize = sizeof(iv::Point2D);
    int i;
    for(i=0;i<nplansize;i++)
    {
        iv::Point2D x;
        memcpy(&x,strdata + i*npsize,npsize);
        m_plan_left.push_back(x);
    }
    mMutexPlan_left.unlock();
}

void ADCIntelligentVehicle::UpdatePlanTrace_right(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    mMutexPlan_right.lock();
    m_plan_right.clear();
    int nplansize = nSize/sizeof(iv::Point2D);
    if(nplansize < 1)
        std::cout<<"UpdatePlanTrace_right size is 0................. "<<std::endl;
    int npsize = sizeof(iv::Point2D);
    int i;
    for(i=0;i<nplansize;i++)
    {
        iv::Point2D x;
        memcpy(&x,strdata + i*npsize,npsize);
        m_plan_right.push_back(x);
    }
    mMutexPlan_right.unlock();
}


void ADCIntelligentVehicle::UpdateFusion(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::fusion::fusionobjectarray xfusionarray;
    if(!xfusionarray.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateFusion Parse Error.");
        return;
    }
    mMutexFusion.lock();
    mfusionarray.CopyFrom(xfusionarray);
    mbfusionUpdate = true;
    mMutexFusion.unlock();

}


/**
 * @brief ADCIntelligentVehicle::UpdateMap
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ADCIntelligentVehicle::UpdateMap(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    //    std::cout<<"update map "<<std::endl;
    int gpsunitsize = sizeof(iv::GPS_INS);
    int nMapSize = nSize/gpsunitsize;
    //    std::cout<<"map size is "<<nMapSize<<std::endl;

    if(nMapSize < 1)return;

    bool bUpdate = false;
    if(nMapSize != m_navigation_data.size())
    {
        bUpdate = true;
    }
    else
    {
        iv::GPS_INS * p = (iv::GPS_INS *)strdata;
        if((p->gps_lat == m_navigation_data.at(0)->gps_lat)&&(p->ins_heading_angle == m_navigation_data.at(0)->ins_heading_angle))
        {
            //           qDebug("same map");
            bUpdate = false;
        }
        else
        {
            bUpdate = true;
        }
    }

    if(bUpdate)
    {
        int i;
        mMutexNavi.lock();
        m_navigation_data.clear();
        for(i=0;i<nMapSize;i++)
        {
            iv::GPS_INS x;
            memcpy(&x,strdata + i*gpsunitsize,gpsunitsize);
            iv::GPSData data(new iv::GPS_INS);
            *data = x;
            m_navigation_data.push_back(data);
        }
        mMutexNavi.unlock();

        if(m_navigation_data.size()>0)
        {
            mDataToUI.mInfo.is_mapLoad = 1;
        }
        else
        {
            mDataToUI.mInfo.is_mapLoad = 0;
        }
    }
    else
    {
        //       qDebug("not need update");
    }
}


//=======================zhaobo0904
#define PI  3.14159265358979

/**
 * @brief GaussProjCal
 * @param lon
 * @param lat
 * @param X
 * @param Y
 */

/*
void GaussProjCal(double lon, double lat, double *X, double *Y)
{
    // 1975 年国际椭球体长半轴 a, 第一离心率 e2, 第二离心率 ep2
    double a = 6378140.0;
    double e2 = 0.006694384999588;
    double ep2 = e2/(1-e2);

    // 原点所在经度
    double lon_origin = 6.0*int(lon/6) + 3.0;
    lon_origin *= PI / 180.0;

    double k0 = 0.9996;

    // 角度转弧度
    double lat1 = lat * PI / 180.0;
    double lon1 = lon * PI / 180.0;


    // 经线在该点处的曲率半径,
    double N = a / sqrt(1 - e2*sin(lat1)*sin(lat1));


    // 赤道到该点的经线长度近似值 M, 使用泰勒展开逐项积分然后取前四项.
    // 这个近似值是将 N 作为纬度 \phi 的函数展开为泰勒计数, 然后在区间 [0, lat1] 上积分得到的.
    // 首先计算前四项的系数 a1~a4.
    double a1 = 1 - e2/4 - (3*e2*e2)/64 - (5*e2*e2*e2)/256;
    double a2 = (3*e2)/8 + (3*e2*e2)/32 + (45*e2*e2*e2)/1024;
    double a3 = (15*e2*e2)/256 + (45*e2*e2*e2)/1024;
    double a4 = (35*e2*e2*e2)/3072;
    double M = a * (a1*lat1 - a2*sin(2*lat1) + a3*sin(4*lat1) - a4*sin(6*lat1));

    // 辅助量
    double T = tan(lat1)*tan(lat1);
    double C = ep2*cos(lat1)*cos(lat1);
    double A = (lon1 - lon_origin)*cos(lat1);

    *X = 500000.0 + k0 * N * (A + (1 - T + C)*(A*A*A)/6 + (5 - 18*T + T*T + 72*C - 58*ep2)*(A*A*A*A*A)/120);
    *Y = M + N * tan(lat1) * ((A*A)/2 +
                              (5 - T + 9*C + 4*C*C)*(A*A*A*A)/24 +
                              (61 - 58*T + T*T + 600*C - 330*ep2)*(A*A*A*A*A*A)/720);


    *Y *= k0;
    return;
}

*/


//高斯投影由经纬度(Unit:DD)反算大地坐标(含带号，Unit:Metres)
void GaussProjCal(double longitude, double latitude, double *X, double *Y)
{
    int ProjNo = 0; int ZoneWide; ////带宽
    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
    double a, f, e2, ee, NN, T, C, A, M, iPI;
    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
    ZoneWide = 6; ////6度带宽
    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
                                    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
    ProjNo = (int)(longitude / ZoneWide);
    longitude0 = ProjNo * ZoneWide + ZoneWide / 2;
    longitude0 = longitude0 * iPI;
    latitude0 = 0;
    longitude1 = longitude * iPI; //经度转换为弧度
    latitude1 = latitude * iPI; //纬度转换为弧度
    e2 = 2 * f - f * f;
    ee = e2 * (1.0 - e2);
    NN = a / sqrt(1.0 - e2 * sin(latitude1)*sin(latitude1));
    T = tan(latitude1)*tan(latitude1);
    C = ee * cos(latitude1)*cos(latitude1);
    A = (longitude1 - longitude0)*cos(latitude1);
    M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2
        *e2 / 1024)*sin(2 * latitude1)
        + (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*sin(6 * latitude1));
    xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120);
    yval = M + NN * tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24
        + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720);
    X0 = 1000000L * (ProjNo + 1) + 500000L;
    Y0 = 0;
    xval = xval + X0; yval = yval + Y0;
    *X = xval;
    *Y = yval;
}



//高斯投影由大地坐标(Unit:Metres)反算经纬度(Unit:DD)
void GaussProjInvCal(double X, double Y, double *longitude, double *latitude)
{
    int ProjNo; int ZoneWide; ////带宽
    double longitude1, latitude1, longitude0, latitude0, X0, Y0, xval, yval;
    double e1, e2, f, a, ee, NN, T, C, M, D, R, u, fai, iPI;
    iPI = 0.0174532925199433; ////3.1415926535898/180.0;
    a = 6378245.0; f = 1.0 / 298.3; //54年北京坐标系参数
    ////a=6378140.0; f=1/298.257; //80年西安坐标系参数
    ZoneWide = 6; ////6度带宽
    ProjNo = (int)(X / 1000000L); //查找带号
    longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2;
    longitude0 = longitude0 * iPI; //中央经线
    X0 = ProjNo * 1000000L + 500000L;
    Y0 = 0;
    xval = X - X0; yval = Y - Y0; //带内大地坐标
    e2 = 2 * f - f * f;
    e1 = (1.0 - sqrt(1 - e2)) / (1.0 + sqrt(1 - e2));
    ee = e2 / (1 - e2);
    M = yval;
    u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256));
    fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*sin(
                4 * u)
            + (151 * e1*e1*e1 / 96)*sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*sin(8 * u);
    C = ee * cos(fai)*cos(fai);
    T = tan(fai)*tan(fai);
    NN = a / sqrt(1.0 - e2 * sin(fai)*sin(fai));
    R = a * (1 - e2) / sqrt((1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin(fai)*sin(fai))*(1 - e2 * sin
                                                                                       (fai)*sin(fai)));
    D = xval / NN;
    //计算经度(Longitude) 纬度(Latitude)
    longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D
                               *D*D*D*D / 120) / cos(fai);
    latitude1 = fai - (NN*tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24
                                         + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720);
    //转换为度 DD
    *longitude = longitude1 / iPI;
    *latitude = latitude1 / iPI;
}


//==========================================================

/**
 * @brief ADCIntelligentVehicle::UpdateGPSIMU
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */

void ADCIntelligentVehicle::UpdateGPSIMU(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    iv::gps::gpsimu  xgpsimu;
    if(!xgpsimu.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateGPSIMU parse error. nSize is %d",nSize);
        return;
    }



    iv::GPSData data(new iv::GPS_INS);

    data->gps_lat = xgpsimu.lat();
    data->gps_lng = xgpsimu.lon();
    data->ins_heading_angle = xgpsimu.heading();
    data->rtk_status = xgpsimu.rtk_state();
    data->ins_status = xgpsimu.ins_state();
    data->vel_D = xgpsimu.vd();    //地向速度,单位(米/秒)
    data->vel_E = xgpsimu.ve();    //东向速度,单位(米/秒)
    data->vel_N = xgpsimu.vn();    //北向速度,单位(米/秒)


    GaussProjCal(data->gps_lng,data->gps_lat,&data->gps_x,&data->gps_y);


    ServiceCarStatus.mRTKStatus = data->rtk_status;

    ServiceCarStatus.speed = data->speed;

    ServiceCarStatus.location->gps_lat = data->gps_lat;
    ServiceCarStatus.location->gps_lng = data->gps_lng;
    ServiceCarStatus.location->ins_heading_angle = data->ins_heading_angle;
    ServiceCarStatus.location->gps_x = data->gps_x;
    ServiceCarStatus.location->gps_y = data->gps_y;
    ServiceCarStatus.location->rtk_status = data->rtk_status;
    ServiceCarStatus.location->ins_status = data->ins_status;

    ServiceCarStatus.speed = sqrt(pow(data->vel_E,2)+pow(data->vel_N,2)) * 3.6;  //double pow(double x, double y) 返回 x 的 y 次幂，即 xy。
    ui->lineEdit_lon->setText(QString::number(ServiceCarStatus.location->gps_lng,'f',7));
    ui->lineEdit_lat->setText(QString::number(ServiceCarStatus.location->gps_lat,'f',7));
    ui->lineEdit_heading->setText(QString::number(ServiceCarStatus.location->ins_heading_angle,'f',1));
    mnTimeUpdateGPS = mTimeState.elapsed();
}

/**
 * @brief ADCIntelligentVehicle::UpdateRADAR
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ADCIntelligentVehicle::UpdateRADAR(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    static qint64 oldrecvtime;
    iv::radar::radarobjectarray xradararray;
    if(!xradararray.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateRADAR Parse Error.");
        return;
    }

 //   gIvlog->verbose("radar time is %ld",QDateTime::currentMSecsSinceEpoch());

    if((QDateTime::currentMSecsSinceEpoch() - oldrecvtime)>100)
    {
        gIvlog->warn("radar interval is more than 100ms.  value is %ld",QDateTime::currentMSecsSinceEpoch() - oldrecvtime);
    }

    oldrecvtime = QDateTime::currentMSecsSinceEpoch();

    mMutexRadar.lock();
    mradararray.CopyFrom(xradararray);
    mbradarUpdate = true;
    mMutexRadar.unlock();
    //    int i;
    //    for(i=0;i<64;i++)
    //    {
    //        iv::ObstacleBasic x;
    //        memcpy(&x,pdata + i*sizeof(iv::ObstacleBasic),sizeof(iv::ObstacleBasic));
    //        ServiceCarStatus.obs_radar[i] = x;
    //    }
        ServiceCarStatus.mRadarS = 10;
    //    mnTimeUpdateRadar = mTimeState.elapsed();
    //    mnTimeUpdateCANState = mTimeState.elapsed();
}

/**
 * @brief ADCIntelligentVehicle::UpdateCANState
 * @param pdata
 * @param ndatasize
 */
void ADCIntelligentVehicle::UpdateCANState(const char *pdata, const int ndatasize)
{
    iv::canstate::canstate recCanstate;
    if(!recCanstate.ParseFromArray(pdata, ndatasize))
    {
        gIvlog->warn("ADCIntelligentVehicle::updateCanstate error");
        return;
    }

//    if(ndatasize<sizeof(iv::can_state))return;
//    char ccanstate[ndatasize];

//    memcpy(ccanstate,pdata,4);
#if 1
    if(recCanstate.b_canstate())
    {        
//        gIvlog->debug("UpdateCANState<success>");
        mDataToUI.mInfo.is_initSuccess = 1;
    }
    else
    {
//        gIvlog->debug("UpdateCANState<failed>size %d %s ");
        mDataToUI.mInfo.is_initSuccess = 0;
    }
#endif



//    iv::can_state x;
//    memcpy(&x,pdata,sizeof(iv::can_state));

//    if(x.mbCANOpen)
//    {
//        mDataToUI.mInfo.is_initSuccess = 1;
//    }
//    else
//    {
//        mDataToUI.mInfo.is_initSuccess = 0;
//    }

}

/**
 * @brief ADCIntelligentVehicle::UpdateDecition
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ADCIntelligentVehicle::UpdateDecition(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{

    iv::brain::decition xdecition;
    if(!xdecition.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateDecition parse errror. nSize is %d",nSize);
        return;
    }
    //ServiceCarStatus.mfAcc = xdecition.accelerator();   //
    ServiceCarStatus.mfAcc = xdecition.torque();          //
    ServiceCarStatus.mfBrake = xdecition.brake();
    ServiceCarStatus.mfWheel = xdecition.wheelangle();
    ServiceCarStatus.mfttc = xdecition.ttc();

    mnTimeUpdateDecition = mTimeState.elapsed();
}

/**
 * @brief ADCIntelligentVehicle::UpdateVehicleState
 * @param strdata
 * @param nSize
 * @param index
 * @param dt
 * @param strmemname
 */
void ADCIntelligentVehicle::UpdateVehicleState(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::brain::brainstate xbrainstate;
    if(!xbrainstate.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateVehicleState parse error,nSize is %d ",nSize);
        return;
    }
    ServiceCarStatus.bocheEnable = xbrainstate.mbbocheenable();
    ServiceCarStatus.mObs = xbrainstate.mfobs();
    ServiceCarStatus.mLidarObs = xbrainstate.mflidarobs();
    ServiceCarStatus.mRadarObs = xbrainstate.mfradarobs();
    mbBrainRunning = xbrainstate.mbbrainrunning();
    ServiceCarStatus.mbBrainCtring = xbrainstate.mbbrainrunning();
    mdecition_period =  xbrainstate.decision_period();

    mnTimeUpdateVS = mTimeState.elapsed();
}

/**
 * @brief ADCIntelligentVehicle::UpdateOBS
 * @param lidar_obs
 */
void ADCIntelligentVehicle::UpdateOBS(std::shared_ptr<std::vector<iv::ObstacleBasic> > lidar_obs)
{
    iv::ObsLiDAR _obs_grid_ = boost::shared_ptr<std::vector<iv::ObstacleBasic>>(new std::vector<iv::ObstacleBasic>);
    int i;
    int nSize = lidar_obs->size();
    for(i=0;i<nSize;i++)
        _obs_grid_->push_back(lidar_obs->at(i));
    ServiceLidar.copyfromlidarobs(_obs_grid_);

    ServiceCarStatus.mLidarS = 10;
}

void ADCIntelligentVehicle::UpdateV2xStEn(unsigned int enable)
{
    iv::v2x::v2xStEn x;
    x.set_v2xsten(enable);
    int nsize = x.ByteSize();
    char * str = new char[nsize];
    if(x.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mp_v2xStSend,str,nsize);
    }
    else
    {
        gIvlog->error("hmi","send enable st error");
    }
    delete str;
}

/**
 * @brief ADCIntelligentVehicle::ShareHMIMsg
 * @param xhmi
 */
void ADCIntelligentVehicle::ShareHMIMsg(iv::hmi::HMIBasic xhmi)
{
    iv::hmi::hmimsg xhmimsg;
    xhmimsg.set_mbpause(xhmi.mbPause);
    xhmimsg.set_mbbusmode(xhmi.mbbusmode);
    xhmimsg.set_mbbochemode(xhmi.mbBocheMode);
    xhmimsg.set_mbchemen(xhmi.mbchemen);
    xhmimsg.set_mbjinguang(xhmi.mbjinguang);
    xhmimsg.set_mbyuanguang(xhmi.mbyuanguang);
    int nsize = xhmimsg.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);   //将就对象的引用计数减1（当然，如果发现引用计数为0时，则析构旧对象），然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。
    if(!xhmimsg.SerializeToArray(str,nsize))
    {
        std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<<std::endl;
        mpivlog->error("ADCIntelligentVehicle::ShareHMIMsg serialize error.");
        return;
    }
    iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize);
}

/**
 * @brief ADCIntelligentVehicle::ShareHMIMsg
 * @param xhmi
 */
void ADCIntelligentVehicle::ShareHMIMsgPro(iv::hmi::hmimsg xhmimsg)
{
    int nsize = xhmimsg.ByteSize();
    char * str = new char[nsize];
    std::shared_ptr<char> pstr;
    pstr.reset(str);   //将就对象的引用计数减1（当然，如果发现引用计数为0时，则析构旧对象），然后将新对象的指针交给智能指针保管。这样做可以省略了delete的过程。
    if(!xhmimsg.SerializeToArray(str,nsize))
    {
        std::cout<<"ADCIntelligentVehicle::ShareHMIMsg serialize error."<<std::endl;
        mpivlog->error("ADCIntelligentVehicle::ShareHMIMsg serialize error.");
        return;
    }
    iv::modulecomm::ModuleSendMsg(mpaHMI,str,nsize);
}

void ADCIntelligentVehicle::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.has_epsmode())&&(xchassis.epsmode() == 0))
    {
        ServiceCarStatus.mbRunPause = true;
    }

    if((xchassis.has_soc()))
    {
        mfSOC = xchassis.soc();
        ui->lineEdit_soc->setText(QString::number(mfSOC,'f',1));
    }
    unsigned char errSig = 0;
    if((xchassis.has_driver_error()))
    {
        errSig |= xchassis.driver_error();
    }
    if((xchassis.has_swj_error()))
    {
        errSig |= xchassis.swj_error();
    }
    if((xchassis.has_battery_error()))
    {
        errSig |= xchassis.battery_error();
    }
    if((xchassis.has_remote_error()))
    {
        errSig |= xchassis.remote_error();
    }
    if((xchassis.has_steer_motor_error()))
    {
        errSig |= xchassis.steer_motor_error();
    }
    if((xchassis.has_rrmotor_error()))
    {
        errSig |= xchassis.rrmotor_error();
    }
    if((xchassis.has_rlmotor_error()))
    {
        errSig |= xchassis.rlmotor_error();
    }
    if((xchassis.has_fsteer_code_error()))
    {
        errSig |= xchassis.fsteer_code_error();
    }
    ui->lineEdit_errCode->setText(QString::number(errSig,16));
}

void ADCIntelligentVehicle::onTimerManual()
{
//    if(myview->mPressKeys.count() == 0)return;

    if(mbManual == false)
    {
        if(myview->mPressKeys.contains(Qt::Key_J))
        {
            mbManual = true;
        }
    }
    if(mbManual == true)
    {
        if(myview->mPressKeys.contains(Qt::Key_K))
        {
            mbManual = false;
        }
    }



    if(mbManual == false)return;

    int timediff = mManualTime.elapsed() - mnLastManualTime;
    mnLastManualTime = mManualTime.elapsed();



    if(myview->mPressKeys.contains(Qt::Key_W))
    {
        if(mnCtrlValue[0]<1000)mnCtrlValue[0] = mnCtrlValue[0] + 5 * timediff /20;
    }
    if(myview->mPressKeys.contains(Qt::Key_S))
    {
        mnCtrlValue[0] = 0;
        if(mnCtrlValue[1]<990) mnCtrlValue[1] = mnCtrlValue[1] + 10* timediff /20;
    }
    else
    {
        mnCtrlValue[1] = 0;
    }
    if(myview->mPressKeys.contains(Qt::Key_A))
    {
        if(mnCtrlValue[2]<500)mnCtrlValue[2] = mnCtrlValue[2] + 3* timediff /20;
    }
    if(myview->mPressKeys.contains(Qt::Key_D))
    {
        if(mnCtrlValue[2]>-500)mnCtrlValue[2] = mnCtrlValue[2] - 3* timediff /20;
    }

    iv::modulecomm::ModuleSendMsg(mpManualCtrl,(char *)mnCtrlValue,3*sizeof(int));
}

void ADCIntelligentVehicle::onCtrlManual(bool bCtrl)
{
    mbManual = bCtrl;
    if(bCtrl)
    {
        mTimerManual->start(10);
        mManualTime.start();
        mnLastManualTime = mManualTime.elapsed();
    }
    else
    {
        mTimerManual->stop();
        mnCtrlValue[0] = 0; mnCtrlValue[1] = 0; mnCtrlValue[2] = 0;
    }
}

void ADCIntelligentVehicle::shareMapReqMsg()
{

    iv::map::mapreq x;
    x.set_maptype(1);
    int nsize = x.ByteSize();
    char * str = new char[nsize];
    if(x.SerializeToArray(str,nsize))
    {
        iv::modulecomm::ModuleSendMsg(mpamapreq,str,nsize);
    }
    else
    {
        std::cout<<"iv::map::mapreq serialize error."<<std::endl;
    }
    std::cout<<"iv::map::mapreq serialize OK."<<std::endl;
    delete str;
}

void ADCIntelligentVehicle::UpdateVbox(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
{
    iv::vbox::vbox xvbox;
    if(!xvbox.ParseFromArray(strdata,nSize))
    {
        gIvlog->warn("ADCIntelligentVehicle::UpdateVbox parse errror. nSize is %d",nSize);
        return;
    }
    ServiceCarStatus.st_straight = xvbox.st_straight();
    ServiceCarStatus.st_left = xvbox.st_left();
    ServiceCarStatus.st_right = xvbox.st_right();
    ServiceCarStatus.st_turn = xvbox.st_turn();
    ServiceCarStatus.time_straight = xvbox.time_straight();
    ServiceCarStatus.time_left = xvbox.time_left();
    ServiceCarStatus.time_right = xvbox.time_right();
    ServiceCarStatus.time_turn = xvbox.time_turn();


}

void ADCIntelligentVehicle::on_button_start_clicked()
{
#ifdef USE_PAD_CTRL
    ServiceCarStatus.mbRunPause = false;
    iv::hmi::HMIBasic hmi;
    hmi.mbBocheMode = false;
    hmi.mbPause = false;
    ShareHMIMsg(hmi);
    return;
#endif
}
//云平台使能控制
void ADCIntelligentVehicle::on_button_platform_en_clicked()
{
    if(mplatformEn){
        ui->button_platform_en->setStyleSheet("background-color: gray");
        mplatformEn = 0;
    }
    else{
        ui->button_platform_en->setStyleSheet("background-color: green");
        mplatformEn = 1;
    }
    gIvlog->info("hmi","platform enable:%d", mplatformEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    hmi.set_platformen(mplatformEn);
    ShareHMIMsgPro(hmi);
}
//路测设备使能控制
void ADCIntelligentVehicle::on_button_obu_en_clicked()
{
    if(mobuEn){
        ui->button_obu_en->setStyleSheet("background-color: gray");
        mobuEn = 0;
    }
    else{
        ui->button_obu_en->setStyleSheet("background-color: green");
        mobuEn = 1;
    }
    gIvlog->info("hmi","platform enable:%d", mobuEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    hmi.set_obuen(mobuEn);
    ShareHMIMsgPro(hmi);
}

void ADCIntelligentVehicle::on_button_map_set_clicked()
{
     QString fileName = QFileDialog::getOpenFileName(this,
                                 tr("选择地图文件"),
                                 "/home/nvidia",
                                 tr("Text Files (*.xodr)"));
     if (fileName.isEmpty())
     {
         QMessageBox msgBox(QMessageBox::Warning, tr("警告"),
                                "No Map Selected", 0, this);
         msgBox.addButton(tr("OK"), QMessageBox::AcceptRole);
//         msgBox.addButton(tr("&Continue"), QMessageBox::RejectRole);
//         if (msgBox.exec() == QMessageBox::AcceptRole)
//             qDebug()<<"accept";
//         else
//             qDebug()<<"reject";
    }
}

void ADCIntelligentVehicle::on_button_vin_set_clicked()
{
    bool ok;
    QString text = QInputDialog::getText(this, tr("输入车辆VIN："),
                                         tr("VIN:"), QLineEdit::Normal,
                                         QDir::home().dirName(), &ok);
    if (ok && !text.isEmpty())
    {
        gstrvin = text.toStdString();
        iv::hmi::hmimsg hmi;
        hmi.set_vin(gstrvin);
        ShareHMIMsgPro(hmi);
        ui->lineEdit_vin->setText(text);
    }
}

void ADCIntelligentVehicle::on_button_speedLimit_set_clicked()
{
    bool ok;
    double d = QInputDialog::getDouble(this, tr("QInputDialog::getDouble()"),
                                       tr("Amount:"), 0.0, 0, 30, 1, &ok);
    if (ok)
    {
//        doubleLabel->setText(QString("$%1").arg(d));
        mdspeedLimit = d;
        iv::hmi::hmimsg hmi;
        hmi.set_speedlimit(mdspeedLimit);
        ShareHMIMsgPro(hmi);
        ui->lineEdit_speedLimit->setText(QString::number(mdspeedLimit,'f',1));
    }
}
void ADCIntelligentVehicle::buttonColorReset()
{
    ui->button_trafficInfo_en->setStyleSheet("background-color: gray");
    ui->button_FCW_en->setStyleSheet("background-color: gray");
    ui->button_jams_en->setStyleSheet("background-color: gray");
    ui->button_DriCrims_en->setStyleSheet("background-color: gray");
}

void ADCIntelligentVehicle::on_button_trafficInfo_en_clicked()
{
    buttonColorReset();
    if(mbTrafficInfoEn){
        ui->button_trafficInfo_en->setStyleSheet("background-color: gray");
        mbTrafficInfoEn = false;
    }
    else{
        ui->button_trafficInfo_en->setStyleSheet("background-color: green");
        mbTrafficInfoEn = true;
    }
    gIvlog->info("hmi","traffic info enable:%d", mbTrafficInfoEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    mbFCWEn = false;
    mbjamsEn = false;
    mbdriCrimsEn = false;
    hmi.set_rodeinfoen(mbTrafficInfoEn);
    hmi.set_rodefcwen(mbFCWEn);
    hmi.set_roadjamsen(mbjamsEn);
    hmi.set_roaddricrimsen(mbdriCrimsEn);
    ShareHMIMsgPro(hmi);
}

void ADCIntelligentVehicle::on_button_FCW_en_clicked()
{
    buttonColorReset();
    if(mbFCWEn){
        ui->button_FCW_en->setStyleSheet("background-color: gray");
        mbFCWEn = false;
    }
    else{
        ui->button_FCW_en->setStyleSheet("background-color: green");
        mbFCWEn = true;
    }
    gIvlog->info("hmi","traffic FCW enable:%d", mbFCWEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    mbTrafficInfoEn = false;
    mbjamsEn = false;
    mbdriCrimsEn = false;
    hmi.set_rodeinfoen(mbTrafficInfoEn);
    hmi.set_rodefcwen(mbFCWEn);
    hmi.set_roadjamsen(mbjamsEn);
    hmi.set_roaddricrimsen(mbdriCrimsEn);
    ShareHMIMsgPro(hmi);
}

void ADCIntelligentVehicle::on_button_jams_en_clicked()
{
    buttonColorReset();
    if(mbjamsEn){
        ui->button_jams_en->setStyleSheet("background-color: gray");
        mbjamsEn = false;
    }
    else{
        ui->button_jams_en->setStyleSheet("background-color: green");
        mbjamsEn = true;
    }
    gIvlog->info("hmi","traffic jams enable:%d", mbjamsEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    mbTrafficInfoEn = false;
    mbFCWEn = false;
    mbdriCrimsEn = false;
    hmi.set_rodeinfoen(mbTrafficInfoEn);
    hmi.set_rodefcwen(mbFCWEn);
    hmi.set_roadjamsen(mbjamsEn);
    hmi.set_roaddricrimsen(mbdriCrimsEn);
    ShareHMIMsgPro(hmi);
}

void ADCIntelligentVehicle::on_button_SimCar_en_clicked()
{
    iv::hmi::hmimsg hmi;
    hmi.set_lonup(ui->lineEdit_jamsLon_up->text().toDouble());
    hmi.set_lonlow(ui->lineEdit_jamsLon_low->text().toDouble());
    hmi.set_latup(ui->lineEdit_jamsLat_Up->text().toDouble());
    hmi.set_latlow(ui->lineEdit_jamsLat_low->text().toDouble());
    hmi.set_headingup(ui->lineEdit_jamsHead_up->text().toDouble());
    hmi.set_headinglow(ui->lineEdit_jamsHead_low->text().toDouble());
    hmi.set_carcount(ui->lineEdit_jamsCarNum->text().toInt());
    ShareHMIMsgPro(hmi);
}

void ADCIntelligentVehicle::on_button_DriCrims_en_clicked()
{
    buttonColorReset();
    if(mbdriCrimsEn){
        ui->button_DriCrims_en->setStyleSheet("background-color: gray");
        mbdriCrimsEn = false;
    }
    else{
        ui->button_DriCrims_en->setStyleSheet("background-color: green");
        mbdriCrimsEn = true;
    }
    gIvlog->info("hmi","traffic jams enable:%d", mbdriCrimsEn);
//    UpdateV2xStEn(mv2xStEn);
    iv::hmi::hmimsg hmi;
    mbTrafficInfoEn = false;
    mbFCWEn = false;
    mbjamsEn = false;
    hmi.set_rodeinfoen(mbTrafficInfoEn);
    hmi.set_rodefcwen(mbFCWEn);
    hmi.set_roadjamsen(mbjamsEn);
    hmi.set_roaddricrimsen(mbdriCrimsEn);
    ShareHMIMsgPro(hmi);
}