Add car status

src/driver/driver_cloud_grpc_client_BS/VehicleUpload.proto

@@ -47,6 +47,10 @@ message GPSPoint{
 enum UseStatus{
     DEACTIVATING = 0;//停用中
     ENABLING = 1;//启用中
+    IN_THE_MISSION = 2;//ÈÎÎñ;ÖÐ
+    REMOTE_CTRL = 3;//È˹¤½Ó¹Ü״̬
+    BACK_TO_WAIT_STATION = 4;//»Øµ½´ýÃüµã;ÖÐ
+    BACK_TO_MAINTAIN_STATION = 5;//»Øµ½³äµçµã;ÖÐ
 }
 
 enum VehicleClass{

src/driver/driver_cloud_grpc_client_BS/driver_cloud_grpc_client_BS.pro

@@ -58,6 +58,7 @@ else: unix:!android: target.path = /opt/$${TARGET}/bin
     error( "Couldn't find the ivprotobuf.pri file!" )
 }
 
+
 !include(../../../include/ivboost.pri ) {
     error( "Couldn't find the ivboost.pri file!" )
 }

src/driver/driver_cloud_grpc_client_BS/fsm_unit.cpp

@@ -289,7 +289,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             waitingStationArrived = false;
         }
-        emit useStatusChanged(true);
+        emit useStatusChanged(1);
         emit setAllowPlan(true);
         break;
     case 1: ///< 1任务中
@@ -301,6 +301,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(2);
         emit setAllowPlan(false);
         break;
     case 2: ///< 2人工接管
@@ -308,6 +309,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(3);
         emit setAllowPlan(false);
         break;
     case 3: ///< 3停工停车
@@ -319,7 +321,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             maintainStationArrived = false;
         }
-        emit useStatusChanged(false);
+        emit useStatusChanged(0);
         emit setAllowPlan(false);
         break;
     case 4: ///< 4返程
@@ -327,6 +329,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(4);
         emit setAllowPlan(false);
         break;
     case 5: ///< 5返库
@@ -338,6 +341,7 @@ void FSMUnit::refreshFSMStatus_Slot(void)
         {
             remoteCtrlCMD = iv::brain::RemoteCtrlCMD::REMOTE_CTRL_RESERVED;
         }
+        emit useStatusChanged(5);
         emit setAllowPlan(false);
         break;
     default:

src/driver/driver_cloud_grpc_client_BS/fsm_unit.h

@@ -100,7 +100,7 @@ private:
 
 signals:
     void refreshFSMStatus(void);
-    void useStatusChanged(bool status);
+    void useStatusChanged(int status);
     void setAllowPlan(bool isAllow);
     void sendPathPlanRequest(double latitude,double longitude);
 

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -390,7 +390,7 @@ void DataExchangeClient::ListenChassisMsg(const char * strdata,const unsigned in
     gMutex_Chassis.lock();
     SOC = xdata.soc();
     gMutex_Chassis.unlock();
-//    std::cout<<SOC<<std::endl;
+    //    std::cout<<SOC<<std::endl;
 }
 
 void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
@@ -402,8 +402,8 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
         return;
     }
 
-//    double GPS_Speed = sqrt(xdata.ve()*xdata.ve()+xdata.vd()*xdata.vd()+xdata.vn()*xdata.vn());
-//    if (GPS_Speed < 0.01)GPS_Speed = 0.0;
+    //    double GPS_Speed = sqrt(xdata.ve()*xdata.ve()+xdata.vd()*xdata.vd()+xdata.vn()*xdata.vn());
+    //    if (GPS_Speed < 0.01)GPS_Speed = 0.0;
 
     gMutex_GPSIMU.lock();
     GPSRTKStatus = xdata.rtk_state();
@@ -414,10 +414,10 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
     roll = xdata.roll();
     heading = xdata.heading();
     speed = xdata.speed();
-//    std::cout<<"speed: "<<speed<<std::endl;
+    //    std::cout<<"speed: "<<speed<<std::endl;
     if(speed < 0.01)speed = 0.0;
     gMutex_GPSIMU.unlock();
-//    std::cout<<heading<<std::endl;
+    //    std::cout<<heading<<std::endl;
 }
 
 void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
@@ -480,12 +480,12 @@ void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const un
     brakeFeedback = xdata.brake();
     steeringWheelAngleFeedback = (-1) * xdata.steeringwheelangle();
     gMutex_PlatformFeedback.unlock();
-//    std::cout<<throttleFeedback<<","<<brakeFeedback<<","<<steeringWheelAngleFeedback<<","<<shiftFeedback<<","<<modeFeedback<<std::endl;
+    //    std::cout<<throttleFeedback<<","<<brakeFeedback<<","<<steeringWheelAngleFeedback<<","<<shiftFeedback<<","<<modeFeedback<<std::endl;
 }
 
 void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
 {
-// recive trance map
+    // recive trance map
     int npoint = nSize/sizeof(iv::simpletrace);
     if(npoint < 1)
     {
@@ -505,7 +505,7 @@ void DataExchangeClient::ListenTraceMapMsg(const char * strdata,const unsigned i
         onePoint.mutable_mappoint()->set_latitude(simpletrace[i].gps_lat);
         onePoint.mutable_mappoint()->set_longitude(simpletrace[i].gps_lng);
         pathPoints.append(onePoint);
-//        std::cout<<pathPoints.at(i).index()<<std::endl;
+        //        std::cout<<pathPoints.at(i).index()<<std::endl;
     }
     emit setFSMDestination(pathPoints.at(pathPoints.size()-1).mappoint().latitude(),pathPoints.at(pathPoints.size()-1).mappoint().longitude());
     std::cout<<"get a tracemap"<<std::endl;
@@ -561,20 +561,20 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_roll(roll);
     request.set_heading(heading);
     gMutex_GPSIMU.unlock();
-//  std::cout<<std::setprecision(12)<<"vehicle position: "<<positionFeedback.latitude()<<","<<positionFeedback.longitude()<<std::endl;
-
-//    gMutex_ImageFront.lock();
-//    request.set_cameraimagefront(cameraImageFront.data(),cameraImageFront.size());
-//    gMutex_ImageFront.unlock();
-//    gMutex_ImageRear.lock();
-//    request.set_cameraimagerear(cameraImageRear.data(),cameraImageRear.size());
-//    gMutex_ImageRear.unlock();
-//    gMutex_ImageLeft.lock();
-//    request.set_cameraimageleft(cameraImageLeft.data(),cameraImageLeft.size());
-//    gMutex_ImageLeft.unlock();
-//    gMutex_ImageRight.lock();
-//    request.set_cameraimageright(cameraImageRight.data(),cameraImageRight.size());
-//    gMutex_ImageRight.unlock();
+    //  std::cout<<std::setprecision(12)<<"vehicle position: "<<positionFeedback.latitude()<<","<<positionFeedback.longitude()<<std::endl;
+
+    //    gMutex_ImageFront.lock();
+    //    request.set_cameraimagefront(cameraImageFront.data(),cameraImageFront.size());
+    //    gMutex_ImageFront.unlock();
+    //    gMutex_ImageRear.lock();
+    //    request.set_cameraimagerear(cameraImageRear.data(),cameraImageRear.size());
+    //    gMutex_ImageRear.unlock();
+    //    gMutex_ImageLeft.lock();
+    //    request.set_cameraimageleft(cameraImageLeft.data(),cameraImageLeft.size());
+    //    gMutex_ImageLeft.unlock();
+    //    gMutex_ImageRight.lock();
+    //    request.set_cameraimageright(cameraImageRight.data(),cameraImageRight.size());
+    //    gMutex_ImageRight.unlock();
 
     request.set_sensorstatusgpsimu(sensorStatusGPSIMU);
     request.set_sensorstatuslidar(sensorStatusLidar);
@@ -583,16 +583,16 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     request.set_sensorstatuscamrear(sensorStatusCamRear);
     request.set_sensorstatuscamleft(sensorStatusCamLeft);
     request.set_sensorstatuscamright(sensorStatusCamRight);
-//    if(isArrived == 1)
-//    {
-//        if(gNavagationSwitch == 0)
-//            request.set_isarrived(0);
-//        if(gNavagationSwitch == 1)
-//            request.set_isarrived(isArrived);
-//    }
-//    else
+    //    if(isArrived == 1)
+    //    {
+    //        if(gNavagationSwitch == 0)
+    //            request.set_isarrived(0);
+    //        if(gNavagationSwitch == 1)
+    //            request.set_isarrived(isArrived);
+    //    }
+    //    else
     request.set_isarrived(isArrived);
-//    std::cout<<"isarrived: "<<isArrived<<std::endl;
+    //    std::cout<<"isarrived: "<<isArrived<<std::endl;
     request.set_platenumber(plateNumber);
     request.set_usestatusfeedback(useStatus);
     request.set_remainpathlength(remainPathLength);
@@ -617,7 +617,7 @@ std::string DataExchangeClient::uploadVehicleInfo(void)
     if (status.ok()) {
         destinationPosition.CopyFrom(reply.destinationposition());
         emit destination_Recieved();
-//        std::cout<<std::setprecision(12)<<"lat:"<<reply.destinationposition().latitude()<<"lon:"<<reply.destinationposition().longitude()<<"height:"<<reply.destinationposition().height()<<std::endl;
+        //        std::cout<<std::setprecision(12)<<"lat:"<<reply.destinationposition().latitude()<<"lon:"<<reply.destinationposition().longitude()<<"height:"<<reply.destinationposition().height()<<std::endl;
         return "uploadVehicleInfo RPC successed";
     } else {
         std::cout << status.error_code() << ": " << status.error_message()
@@ -684,54 +684,54 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     org::jeecg::defsDetails::grpc::GPSPoint tempPosition;
 
     timeStamp = QDateTime::currentMSecsSinceEpoch();
-//    SOC = 87.5;
-//    statusFeedback = VehicleStatus::STATUS_REMOTE;
+    //    SOC = 87.5;
+    //    statusFeedback = VehicleStatus::STATUS_REMOTE;
     gMutex_GPSIMU.lock();
     mileage += (speed * timeInterval_ms/1000.0)/1000.0; // kilometer
     tempPosition.CopyFrom(positionFeedback);
-//    std::cout<<std::setprecision(12)<<tempPosition.latitude()<<","<<tempPosition.longitude()<<std::endl;
+    //    std::cout<<std::setprecision(12)<<tempPosition.latitude()<<","<<tempPosition.longitude()<<std::endl;
     gMutex_GPSIMU.unlock();
-//    speed = 0.0; // m/s
-//    shiftFeedback = ShiftStatus::SHIFT_DRIVE;
-//    steeringWheelAngleFeedback = 1.23; //+/-540 degree
-//    throttleFeedback = 0.12;
-//    brakeFeedback = 0.34;
-//    GPSRTKStatus = 6; //GPS-RTK status 0-6 6 is best
-//    positionFeedback.set_latitude(39.0666552);
-//    positionFeedback.set_longitude(117.3540963);
-//    positionFeedback.set_height(0.84);
-//    pitch = 0.03;
-//    roll = 0.02;
-//    heading = 198.4;
-
-//    QFile xFile;
-//    xFile.setFileName("/home/nvidia/Pictures/123.jpg");
-//    if(xFile.open(QIODevice::ReadOnly))
-//    {
-//        cameraImageFront = xFile.readAll();
-//    }
-//    xFile.close();
-
-//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
-//    if(xFile.open(QIODevice::ReadOnly))
-//    {
-//        cameraImageRear = xFile.readAll();
-//    }
-//    xFile.close();
-
-//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
-//    if(xFile.open(QIODevice::ReadOnly))
-//    {
-//        cameraImageLeft = xFile.readAll();
-//    }
-//    xFile.close();
-
-//    xFile.setFileName("/home/samuel/Pictures/123.jpg");
-//    if(xFile.open(QIODevice::ReadOnly))
-//    {
-//        cameraImageRight = xFile.readAll();
-//    }
-//    xFile.close();
+    //    speed = 0.0; // m/s
+    //    shiftFeedback = ShiftStatus::SHIFT_DRIVE;
+    //    steeringWheelAngleFeedback = 1.23; //+/-540 degree
+    //    throttleFeedback = 0.12;
+    //    brakeFeedback = 0.34;
+    //    GPSRTKStatus = 6; //GPS-RTK status 0-6 6 is best
+    //    positionFeedback.set_latitude(39.0666552);
+    //    positionFeedback.set_longitude(117.3540963);
+    //    positionFeedback.set_height(0.84);
+    //    pitch = 0.03;
+    //    roll = 0.02;
+    //    heading = 198.4;
+
+    //    QFile xFile;
+    //    xFile.setFileName("/home/nvidia/Pictures/123.jpg");
+    //    if(xFile.open(QIODevice::ReadOnly))
+    //    {
+    //        cameraImageFront = xFile.readAll();
+    //    }
+    //    xFile.close();
+
+    //    xFile.setFileName("/home/samuel/Pictures/123.jpg");
+    //    if(xFile.open(QIODevice::ReadOnly))
+    //    {
+    //        cameraImageRear = xFile.readAll();
+    //    }
+    //    xFile.close();
+
+    //    xFile.setFileName("/home/samuel/Pictures/123.jpg");
+    //    if(xFile.open(QIODevice::ReadOnly))
+    //    {
+    //        cameraImageLeft = xFile.readAll();
+    //    }
+    //    xFile.close();
+
+    //    xFile.setFileName("/home/samuel/Pictures/123.jpg");
+    //    if(xFile.open(QIODevice::ReadOnly))
+    //    {
+    //        cameraImageRight = xFile.readAll();
+    //    }
+    //    xFile.close();
 
     sensorStatusGPSIMU = false; //0 GPS-IMU ok 1 GPS-IMU error
     sensorStatusLidar = false;
@@ -741,7 +741,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
     sensorStatusCamLeft = false;
     sensorStatusCamRight = false;
 
-//    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+    //    useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
     if(useStatus == org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING)
     {
         destinationSaved.Clear();
@@ -765,8 +765,8 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
         double deltaX = localPositionNextX - localPositionX;
         double deltaY = localPositionNextY - localPositionY;
         double distance = sqrt(deltaX*deltaX + deltaY*deltaY);
-//        if(distance < 20.0)
-//            std::cout<<"distance: "<<distance<<std::endl;
+        //        if(distance < 20.0)
+        //            std::cout<<"distance: "<<distance<<std::endl;
         if(distance < 1.5)
         {
             isArrived = 2;
@@ -776,7 +776,7 @@ void DataExchangeClient::updateData(uint64_t timeInterval_ms)
             std::cout<<"destination has arrived"<<std::endl;
         }
     }
-//    std::cout<<"is arrived: "<<isArrived<<std::endl;
+    //    std::cout<<"is arrived: "<<isArrived<<std::endl;
 }
 
 void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points)
@@ -787,7 +787,7 @@ void DataExchangeClient::updatePath(std::string pathID, QVector<MapPoint> points
     for(int i = 0;i < points.size();i++)
     {
         pathPoints.append(points.value(i));
-//        std::cout<<pathPoints.at(i).index()<<std::endl;
+        //        std::cout<<pathPoints.at(i).index()<<std::endl;
     }
 }
 
@@ -831,8 +831,8 @@ void DataExchangeClient::run()
             realLastTime = xTime.elapsed();
             updateData(realInterval);
             std::string reply = uploadVehicleInfo();
-//            std::cout<< reply <<std::endl;
-//            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
+            //            std::cout<< reply <<std::endl;
+            //            std::cout<<std::setprecision(12)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
             lastTime = xTime.elapsed();
         }
         else
@@ -860,42 +860,42 @@ void DataExchangeClient::run()
 
 void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 {
-        isArrived = 0;
-        remainPathLength = 0.0;
-        destinationSaved.Clear();
-        patrolPathID = pathID;
-
-        xodrobj xodrDest;
-        xodrDest.flon = gPOIPoints.at(1).longitude;
-        xodrDest.flat = gPOIPoints.at(1).latitude;
-        xodrDest.lane = 1;
-        std::cout<<"destination: "<<std::setprecision(12)<<gPOIPoints.at(1).longitude<<","<<gPOIPoints.at(1).latitude<<std::endl;
-
-        iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
-//        std::cout<<"modulesend xodr request"<<std::endl;
-        if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
-        std::cout<<"patrol path calculating"<<std::endl;
-
-//    QFile mapfile;
-//    mapfile.setFileName("/home/nvidia/Documents/path2.txt");
-//    QVector<org::jeecg::defsDetails::grpc::MapPoint> somePoints;
-//    if(mapfile.open(QIODevice::ReadOnly | QIODevice::Text))
-//    {
-//        while(!mapfile.atEnd())
-//        {
-//            QByteArray line = mapfile.readLine();
-//            QString map_str(line);
-//            QStringList oneline = map_str.split(",");
-//            org::jeecg::defsDetails::grpc::MapPoint onePoint;
-//            onePoint.set_index(oneline.at(0).toInt());
-//            onePoint.mutable_mappoint()->set_longitude(oneline.at(1).toDouble());
-//            onePoint.mutable_mappoint()->set_latitude(oneline.at(2).toDouble());
-//            onePoint.mutable_mappoint()->set_height(oneline.at(3).toDouble());
-//            somePoints.append(onePoint);
-//        }
-//    }
-//    updatePath(pathID,somePoints);
-//    uploadPath();
+    isArrived = 0;
+    remainPathLength = 0.0;
+    destinationSaved.Clear();
+    patrolPathID = pathID;
+
+    xodrobj xodrDest;
+    xodrDest.flon = gPOIPoints.at(1).longitude;
+    xodrDest.flat = gPOIPoints.at(1).latitude;
+    xodrDest.lane = 1;
+    std::cout<<"destination: "<<std::setprecision(12)<<gPOIPoints.at(1).longitude<<","<<gPOIPoints.at(1).latitude<<std::endl;
+
+    iv::modulecomm::ModuleSendMsg(shmXodrRequest.mpa,(char *)&xodrDest,sizeof(xodrobj)); ///< send request msg
+    //        std::cout<<"modulesend xodr request"<<std::endl;
+    if(destinationRefreshedTimer.isActive() == false)destinationRefreshedTimer.start(10000);
+    std::cout<<"patrol path calculating"<<std::endl;
+
+    //    QFile mapfile;
+    //    mapfile.setFileName("/home/nvidia/Documents/path2.txt");
+    //    QVector<org::jeecg::defsDetails::grpc::MapPoint> somePoints;
+    //    if(mapfile.open(QIODevice::ReadOnly | QIODevice::Text))
+    //    {
+    //        while(!mapfile.atEnd())
+    //        {
+    //            QByteArray line = mapfile.readLine();
+    //            QString map_str(line);
+    //            QStringList oneline = map_str.split(",");
+    //            org::jeecg::defsDetails::grpc::MapPoint onePoint;
+    //            onePoint.set_index(oneline.at(0).toInt());
+    //            onePoint.mutable_mappoint()->set_longitude(oneline.at(1).toDouble());
+    //            onePoint.mutable_mappoint()->set_latitude(oneline.at(2).toDouble());
+    //            onePoint.mutable_mappoint()->set_height(oneline.at(3).toDouble());
+    //            somePoints.append(onePoint);
+    //        }
+    //    }
+    //    updatePath(pathID,somePoints);
+    //    uploadPath();
 }
 
 void DataExchangeClient::destination_Recieved_Slot(void)
@@ -931,10 +931,29 @@ void DataExchangeClient::destination_Refreshed_Timeout(void)
     std::cout<<"path plan timeout."<<std::endl;
 }
 
-void DataExchangeClient::useStatusChanged_Slot(bool status)
+void DataExchangeClient::useStatusChanged_Slot(int status)
 {
-    if(status)
-        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
-    else
+    switch(status)
+    {
+    case 0:
         useStatus = org::jeecg::defsDetails::grpc::UseStatus::DEACTIVATING;
+        break;
+    case 1:
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::ENABLING;
+        break;
+    case 2:
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::IN_THE_MISSION;
+        break;
+    case 3:
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::REMOTE_CTRL;
+        break;
+    case 4:
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::BACK_TO_WAIT_STATION;
+        break;
+    case 5:
+        useStatus = org::jeecg::defsDetails::grpc::UseStatus::BACK_TO_MAINTAIN_STATION;
+        break;
+    default:
+        break;
+    }
 }

src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -187,7 +187,7 @@ signals:
 
 public slots:
     void patrolPOI_Recieved_Slot(std::string pathID);
-    void useStatusChanged_Slot(bool status);
+    void useStatusChanged_Slot(int status);
 
 private slots:
     void destination_Recieved_Slot(void);