Merge branch 'master' of http://192.168.14.18:3000/adc_pilot/modularization

src/decition/decition_brain/decition/decide_gps_00.cpp

@@ -1758,7 +1758,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
             qDebug("station_nearest: %d, lat: %.7f, lon: %.7f", station_nearest.map_index, station_nearest.station_location.gps_lat, ServiceCarStatus.amilng=station_nearest.station_location.gps_lng);
             givlog->debug("brain_v2x","station_nearest: %d, lat: %.7f, lon: %.7f",
                           station_nearest.map_index, station_nearest.station_location.gps_lat,
-                          ServiceCarStatus.amilng=station_nearest.station_location.gps_lng);
+                          station_nearest.station_location.gps_lng);
             //进入站点模式
             if((ServiceCarStatus.stationCmd.stationStop==0x00))
             {
@@ -1815,20 +1815,11 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
         aim_gps_ins.gps_lat=ServiceCarStatus.amilat;
         aim_gps_ins.gps_lng=ServiceCarStatus.amilng;
-        //   gps2xy(ServiceCarStatus.amilat, ServiceCarStatus.amilng, &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
-        std::cout<<"lng"<<ServiceCarStatus.amilng<<std::endl;
-        qDebug("lat:%f", aim_gps_ins.gps_lat);
-        qDebug("lon:%f", aim_gps_ins.gps_lng);
 
-        std::cout<<"lat"<<ServiceCarStatus.amilat<<std::endl;
+        GaussProjCal( aim_gps_ins.gps_lng, aim_gps_ins.gps_lat,  &aim_gps_ins.gps_x, &aim_gps_ins.gps_y);
         double dis = GetDistance(aim_gps_ins, now_gps_ins);
         Point2D pt = Coordinate_Transfer((aim_gps_ins).gps_x, (aim_gps_ins).gps_y, now_gps_ins);
 
-        std::cout << "\n呼车dis距离:%f\n" << dis << std::endl;
-        std::cout << "\n呼车Y距离:%f\n" << pt.y << std::endl;
-
-        //         if (dis<20 && pt.y<8&& realspeed<0.1)
         if (dis<20 && pt.y<5 && abs(pt.x)<3)
         {
             dSpeed = 0;
@@ -1853,6 +1844,12 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
         {
             dSpeed = min(20.0, dSpeed);
         }
+
+        //站点停车log
+        givlog->debug("brain_v2x","aim_gps_ins.gps_lat: %.7f, aim_gps_ins.gps_lng: %.7f, dis: %.2f, dSpeed: %.1f",
+                      aim_gps_ins.gps_lat, aim_gps_ins.gps_lng,
+                      dis,dSpeed);
+
     }
 
 
@@ -1915,7 +1912,7 @@ iv::decition::Decition iv::decition::DecideGps00::getDecideFromGPS(GPS_INS now_g
 
     if(ServiceCarStatus.stationCmd.mode_manual_drive==true)
     {
-        gps_decition->driveMode=0;
+        gps_decition->mode=0;       //云平台控制切手动驾驶
     }
 
 
@@ -1947,6 +1944,9 @@ iv::Station iv::decition::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins
                 station_n[i].map_index=j;
             }
         }
+        givlog->debug("brain_v2x","stationi: %d, map_index: %d",
+                       i,station_n[i].map_index);
+
     }
     int minDistance=10;
     for (int j = 0; j < gpsMap.size(); j++)
@@ -1958,6 +1958,10 @@ iv::Station iv::decition::DecideGps00::getNearestStation(iv::GPS_INS now_gps_ins
             now_index=j;
         }
     }
+
+    givlog->debug("brain_v2x","now_index: %d",
+                   now_index);
+
     int min_index=gpsMap.size()-1;
     int station_index=0;
     for(int i=0;i<station_size;i++)

src/v2x/v2xTcpClient/data_type.h

@@ -1,54 +1,64 @@
 #ifndef DATA_TYPE_H
 #define DATA_TYPE_H
 /// cameral status, 2019-10-18
+#include <vector>
 enum DownID { StopCommand = 1, AutoPilotControl, StationCommand };
 struct VehicleStatus
 {
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
+    QString head = "'CCFF'";
+    float timeStamp = 0.0;
+    QString vin = "catarc001";
+    int carMode = 1;
+    float carSpeed=0.0;
+    float steerAngle=0.0;
+    int soc=0;
+    int carError=0;
+    int errorNum=0;
+    float lon=0.0;
+    float lat=0.0;
+    float yaw=0.0;
 };
 struct HardwareStatus
 {
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
+    QString head = "'DDFF'";
+    float timeStamp = 0.0;
+    int radarStatus = 0;
+    int radarError = 0;
+    int lidarStatus = 0;
+    int lidarError = 0;
+    int sonicStatus = 0;
+    int sonicError = 0;
+    int cameraStatus = 0;
+    int cameraError = 0;
+    int microphoneStatus = 0;
+    int microphoneError = 0;
+    int gpsStatus = 0;
+    int gpsError = 0;
+    int radarcanStatus = 0;
+    int radarcanError = 0;
+    int canStatus = 0;
+    int canError = 0;
+    int lightStatus = 2;
+    int lightError = 0;
 };
-struct ObstacleData
-{
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
-};
-struct SoftwareStatus
-{
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
-};
-struct EmergencyStop
-{
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
-};
-struct AutoPilotControl
+struct ObstacleDataUnit
 {
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
+    int ObstacleClass = 0;
+    float lon = 0.0;
+    float lat = 0.0;
 };
-struct StationCommand
+struct ObstacleData
 {
-    int cam0 = 0;
-    int cam1 = 0;
-    int cam2 = 0;
-    int cam3 = 0;
+    QString head = "'EEFF'";
+    float timeStamp = 0.0;
+    std::vector<ObstacleDataUnit> obstacles;
 };
+//struct SoftwareStatus
+//{
+//    QString head = "'FFFF'";
+//    float timeStamp = 0.0;
+
+//};
+
+
 #endif // DATA_TYPE_H

src/v2x/v2xTcpClient/mainwindow.cpp

@@ -28,11 +28,39 @@ MainWindow::MainWindow(QWidget *parent) :
     ui(new Ui::MainWindow)
 {
     ui->setupUi(this);
-    socket=new QTcpSocket();    
-    ui->lineEdit_ip->setText("47.95.196.28");
-    ui->lineEdit_port->setText("12123");
-    //连接信号槽
 
+    //Start Get init param
+    QString strpath = QCoreApplication::applicationDirPath();
+    qDebug()<<strpath;
+        strpath = strpath + "/v2xTcpClient.xml";
+
+    iv::xmlparam::Xmlparam xp(strpath.toStdString());
+
+    std::string strCarVIN = xp.GetParam("carVIN","catarc001");
+    std::string strHostIP = xp.GetParam("hostIP","47.95.196.28");
+    std::string strHostPort = xp.GetParam("hostPort","12123");
+    std::string strStationCount = xp.GetParam("stationCount","20");
+    int count = std::stoi(strStationCount,nullptr,10);
+    stationGps location;
+    for(int i = 0; i < count; i++)
+    {
+        std::string strLatName = "lat" + std::to_string(i);
+        std::string strLonName = "lon" + std::to_string(i);
+        std::cout<<strLatName<<strLonName<<std::endl;
+        std::string strLatValue = xp.GetParam(strLatName,"0");
+        std::string strLonValue = xp.GetParam(strLonName,"0");
+
+        location.lat = std::stof(strLatValue,0);
+        location.lon = std::stof(strLonValue,0);
+        mstationGps.append(location);
+    }//END get init param
+
+    socket=new QTcpSocket();
+    ui->lineEdit_ip->setText(QString::fromStdString(strHostIP));
+    ui->lineEdit_port->setText(QString::fromStdString(strHostPort));
+    ui->pushButton_connect->setEnabled(false);
+
+    //连接信号槽
     givlog = new iv::Ivlog("v2x");
     connect(socket, &QTcpSocket::readyRead, this, &MainWindow::socket_Read_Data);
     connect(socket, &QTcpSocket::disconnected, this, &MainWindow::socket_Disconnected);
@@ -41,6 +69,14 @@ MainWindow::MainWindow(QWidget *parent) :
     iv::modulecomm::RegisterRecv("v2xStEn", ListenV2xStEn);
     mp_v2xStSend = iv::modulecomm::RegisterSend("v2xStReq",1000,1);
     shareV2xStReqMsg();
+    //heart beat,jiaolili,20210207
+    m_bEnablePlatform=false;
+    m_bIsConnect=false;
+    QTimer *timer = new QTimer(this);
+    connect(timer,SIGNAL(timeout()),SLOT(heartBeat()));
+    timer->start(1000);
+    ///////////////////////////////////////////
+
 }
 
 MainWindow::~MainWindow()
@@ -48,6 +84,32 @@ MainWindow::~MainWindow()
     delete this->socket;
     delete ui;
 }
+void MainWindow::heartBeat()
+{
+    if(gv2xEn) {
+        if(!m_bEnablePlatform) {
+            connectPlatform();
+        }
+        if(m_bIsConnect) {
+            upDataStream();
+        } else {
+            connectPlatform();
+        }
+
+    } else {
+        if(m_bEnablePlatform) {
+            disconnectPlatform();
+        }
+    }
+    m_bEnablePlatform = gv2xEn;
+}
+void MainWindow::upDataStream()
+{
+//    upVehicleStatus();
+//    upHardwareStatus();
+//   upObstacleDataStatus();
+    upSoftwareStatus();
+}
 void MainWindow::socket_Read_Data()
 {
     QByteArray buffer;
@@ -139,8 +201,8 @@ void MainWindow::ProAutoPilotControl(QStringList list)
                 msgV2xProto.add_stationid(stationId);
 
                 stGps=msgV2xProto.add_stgps();
-                stGps->set_lat(0);
-                stGps->set_lon(0);
+                stGps->set_lat(mstationGps.at(i-4).lat);
+                stGps->set_lon(mstationGps.at(i-4).lon);
                 ui->textEdit_messages->insertPlainText("服务器消息：car station has "+list[i]+"\n");
             }
         } else {
@@ -206,7 +268,40 @@ void MainWindow::socket_Disconnected()
     ui->pushButton_connect->setText("connect");
     ui->textEdit_messages->insertPlainText("服务器消息：Disconnected");
 }
+void MainWindow::connectPlatform()
+{
+    QString IP;
+    int port;
+    //获取IP地址
+    IP = ui->lineEdit_ip->text();
+    //获取端口号
+    port = ui->lineEdit_port->text().toInt();
+    ui->textEdit_messages->setText("");
+    ui->textEdit_messages->insertPlainText("正在连接"+ui->lineEdit_ip->text()+":"+ui->lineEdit_port->text()+"\n");
+    //取消已有的连接
+    socket->abort();
+    //连接服务器
+    socket->connectToHost(IP, port);
+    //等待连接成功
+    if(!socket->waitForConnected(30000)) {
+        ui->textEdit_messages->insertPlainText("连接失败\n");
+        return;
+    }
+    m_bIsConnect = true;
+    ui->textEdit_messages->insertPlainText("连接成功\n");
+    //修改按键文字
+    ui->pushButton_connect->setText("disconnect");
+}
 
+void MainWindow::disconnectPlatform()
+{
+    //断开连接
+    ui->textEdit_messages->setText("断开连接\n");
+    socket->disconnectFromHost();
+    //修改按键文字
+    ui->pushButton_connect->setText("connect");
+    m_bIsConnect=false;
+}
 void MainWindow::on_pushButton_connect_clicked()
 {
     if(ui->pushButton_connect->text() == tr("connect"))
@@ -302,3 +397,35 @@ void MainWindow::shareV2xStReqMsg()
 //    socket->write(bytes);
 //}
 
+void MainWindow::upVehicleStatus()
+{
+    QString time_stamp=getTimeStamp();
+    QString test="['CCFF',"+time_stamp+",'catarc001',1,0,0,100,0,0,0,0,0]";
+    //QByteArray bytes = test.toUtf8();
+    QByteArray bytes = test.toLatin1();
+    socket->write(bytes);
+}
+void MainWindow::upHardwareStatus()
+{
+    QString time_stamp=getTimeStamp();
+    QString test="['DDFF',"+time_stamp+",[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[[0,0],[0,0]],[2,0]]";
+    //QByteArray bytes = test.toUtf8();
+    QByteArray bytes = test.toLatin1();
+    socket->write(bytes);
+}
+void MainWindow::upObstacleDataStatus()
+{
+    QString time_stamp=getTimeStamp();
+    QString test="['EEFF',"+time_stamp+",[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0],[0,0,0]]";
+    //QByteArray bytes = test.toUtf8();
+    QByteArray bytes = test.toLatin1();
+    socket->write(bytes);
+}
+void MainWindow::upSoftwareStatus()
+{
+    QString time_stamp=getTimeStamp();
+    QString test="['FFFF',"+time_stamp+",[1,10,1,10,1,10,1,10],[0,0,0,0,0,0,0,0,0,0,0],[0,0,0,0,0,0]]";
+    //QByteArray bytes = test.toUtf8();
+    QByteArray bytes = test.toLatin1();
+    socket->write(bytes);
+}

src/v2x/v2xTcpClient/mainwindow.h

@@ -12,8 +12,10 @@
 #include "QMessageBox"
 #include "v2x.pb.h"
 #include "modulecomm.h"
+#include "xmlparam.h"
 #include "ivlog.h"
-
+#include <QTimer>
+#include <iostream>
 namespace Ui {
 class MainWindow;
 }
@@ -38,18 +40,28 @@ public:
     void ProStationCommand(QString str);
     bool checkVehicle(QString str);
     int getDownStreamId(QString str);
+    void upVehicleStatus();
+    void upHardwareStatus();
+    void upObstacleDataStatus();
+    void upSoftwareStatus();
+    void upDataStream();
 private slots:
 
     void socket_Read_Data();
     void socket_Disconnected();
     void on_pushButton_connect_clicked();
     void on_textEdit_messages_textChanged();
-
+    void heartBeat();
+    void connectPlatform();
+    void disconnectPlatform();
 private:
     Ui::MainWindow *ui;
     QTcpSocket *socket;
     void * mp_v2xSend;
     void * mp_v2xStSend;
+    bool m_bEnablePlatform;
+    bool m_bIsConnect;
+    QVector<stationGps> mstationGps;
     void shareV2xStReqMsg();
     void shareV2xProtoMsg(iv::v2x::v2x msgV2xProto);
 

src/v2x/v2xTcpClient/v2xTcpClient.xml

@@ -0,0 +1,48 @@
+<xml>	
+	<node name="v2xTcpClient">
+		<param name="carVIN" value="catarc001" />
+		<param name="hostIP" value="47.95.196.28" />
+		<param name="hostPort" value="12123" />
+		<param name="stationCount" value="20" />
+		<param name="lat0" value="" />
+		<param name="lon0" value="" />
+		<param name="lat1" value="" />
+		<param name="lon1" value="" />
+		<param name="lat2" value="" />
+		<param name="lon2" value="" />
+		<param name="lat3" value="" />
+		<param name="lon3" value="" />
+		<param name="lat4" value="" />
+		<param name="lon4" value="" />
+		<param name="lat5" value="" />
+		<param name="lon5" value="" />
+		<param name="lat6" value="" />
+		<param name="lon6" value="" />
+		<param name="lat7" value="" />
+		<param name="lon7" value="" />
+		<param name="lat8" value="" />
+		<param name="lon8" value="" />
+		<param name="lat9" value="" />
+		<param name="lon9" value="" />
+		<param name="lat10" value="" />
+		<param name="lon10" value="" />
+		<param name="lat11" value="" />
+		<param name="lon11" value="" />
+		<param name="lat12" value="" />
+		<param name="lon12" value="" />
+		<param name="lat13" value="" />
+		<param name="lon13" value="" />
+		<param name="lat14" value="" />
+		<param name="lon14" value="" />
+		<param name="lat15" value="" />
+		<param name="lon15" value="" />
+		<param name="lat16" value="" />
+		<param name="lon16" value="" />
+		<param name="lat17" value="" />
+		<param name="lon17" value="" />
+		<param name="lat18" value="" />
+		<param name="lon18" value="" />
+		<param name="lat19" value="" />
+		<param name="lon19" value="" />
+	</node>
+</xml>