Răsfoiți Sursa

fix(grpc_BS):fix string type argument error in sig-slot function

fujiankuan 3 ani în urmă
părinte
comite
a1fa7baf6d

+ 12 - 11
src/driver/driver_cloud_grpc_client_BS/main.cpp

@@ -146,17 +146,18 @@ int main(int argc, char *argv[])
     vehicleuploaddata->start();
 
 //    std::cout<<control_str<<std::endl;
-//    VehicleChangeCtrlModeClient *vehiclechangectrlmode = new VehicleChangeCtrlModeClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
-//    VehicleControlClient *vehiclecontrol = new VehicleControlClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
-//    qRegisterMetaType<org::jeecg::defsControl::grpc::CtrlMode>("org::jeecg::defsControl::grpc::CtrlMode"); ///< to solve the problem of signal and slot being in different threads
-//    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::ctrlMode_Changed,vehiclecontrol,&VehicleControlClient::ctrlMode_Changed_Slot);
-//    vehiclechangectrlmode->start();
-//    vehiclecontrol->start();
-
-//    VehicleUploadMapClient *vehicleuploadmap = new VehicleUploadMapClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
-//    QObject::connect(vehicleuploadmap,&VehicleUploadMapClient::patrolPOI_Recieved,vehicleuploaddata,&DataExchangeClient::patrolPOI_Recieved_Slot);
-//    QObject::connect(vehicleuploaddata,&DataExchangeClient::uploadPath_Finished,vehicleuploadmap,&VehicleUploadMapClient::uploadPath_Finished_Slot);
-//    vehicleuploadmap->start();
+    VehicleChangeCtrlModeClient *vehiclechangectrlmode = new VehicleChangeCtrlModeClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
+    VehicleControlClient *vehiclecontrol = new VehicleControlClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
+    qRegisterMetaType<org::jeecg::defsControl::grpc::CtrlMode>("org::jeecg::defsControl::grpc::CtrlMode"); ///< to solve the problem of signal and slot being in different threads
+    QObject::connect(vehiclechangectrlmode,&VehicleChangeCtrlModeClient::ctrlMode_Changed,vehiclecontrol,&VehicleControlClient::ctrlMode_Changed_Slot);
+    vehiclechangectrlmode->start();
+    vehiclecontrol->start();
+
+    VehicleUploadMapClient *vehicleuploadmap = new VehicleUploadMapClient(grpc::CreateChannel(control_str, grpc::InsecureChannelCredentials()));
+    qRegisterMetaType<std::string>("std::string");
+    QObject::connect(vehicleuploadmap,&VehicleUploadMapClient::patrolPOI_Recieved,vehicleuploaddata,&DataExchangeClient::patrolPOI_Recieved_Slot);
+    QObject::connect(vehicleuploaddata,&DataExchangeClient::uploadPath_Finished,vehicleuploadmap,&VehicleUploadMapClient::uploadPath_Finished_Slot);
+    vehicleuploadmap->start();
 
 //    std::cout<<patrol_str<<std::endl;
 //    VehiclePatrolExceptionClient vehiclepatrol(grpc::CreateChannel(patrol_str, grpc::InsecureChannelCredentials()));

+ 1 - 5
src/driver/driver_cloud_grpc_client_BS/vehicle_control.cpp

@@ -12,7 +12,6 @@ extern std::string gstrplateNumber;
 
 extern char stryamlpath[256];
 
-extern uint8_t gShift_Status; //3 p 4 r 5 n 6 d
 extern uint8_t gCtrlMode_Status; //0 auto 1 remote 2 stop 3 platform
 
 using org::jeecg::defsControl::grpc::Empty; ///< other message
@@ -107,7 +106,6 @@ std::string VehicleControlClient::vehicleControl(void)
             shiftCMD = reply.shiftcmd();
             steeringWheelAngleCMD = reply.steeringwheelanglecmd();
             throttleCMD = reply.throttlecmd();
-            std::cout<<"throttle:"<<reply.throttlecmd()<<std::endl;
             brakeCMD = reply.brakecmd();
         }
         else
@@ -143,8 +141,6 @@ void VehicleControlClient::updateControlData(void)
 //    std::cout<<"throttle:"<<throttleCMD<<std::endl;
 //    std::cout<<"brake:"<<brakeCMD<<"\n"<<std::endl;
 
-    gShift_Status = shiftCMD;
-
     iv::remotectrl xmsg;
     if(modeCMD == CtrlMode::CMD_REMOTE || modeCMD == CtrlMode::CMD_CLOUD_PLATFORM)
         xmsg.set_ntype(iv::remotectrl::CtrlType::remotectrl_CtrlType_REMOTE);
@@ -371,7 +367,7 @@ void VehicleUploadMapClient::run()
             if(isNeedMap == false)
             {
                 std::string reply = uploadMap();
-        //        std::cout<< reply <<std::endl;
+                std::cout<< reply <<std::endl;
                 if(isNeedMap == true)
                 {
                     updateMapPOIData();

+ 33 - 56
src/driver/driver_cloud_grpc_client_BS/vehicle_upload.cpp

@@ -4,6 +4,7 @@
 #include <QString>
 #include <QStringList>
 #include <math.h>
+#include <iomanip>
 
 #include "modulecomm.h"
 #include "rawpic.pb.h"
@@ -29,57 +30,26 @@ using org::jeecg::defsDetails::grpc::MapPoint;
 using org::jeecg::defsDetails::grpc::ShiftStatus; ///< other enum
 using org::jeecg::defsDetails::grpc::CtrlMode;
 
-DataExchangeClient * gDataExchangeClient;
-
-void ListenFrontData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenFrontPicMsg(strdata,nSize);
-}
-
-void ListenRearData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenRearPicMsg(strdata,nSize);
-}
-
-void ListenLeftData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenLeftPicMsg(strdata,nSize);
-}
-
-void ListenRightData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenRightPicMsg(strdata,nSize);
-}
-
-void ListenChassisData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenChassisMsg(strdata,nSize);
-}
-
-void ListenGPSIMUData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenGPSIMUMsg(strdata,nSize);
-}
-
-void ListenPlatformFeedbackData(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
-{
-    gDataExchangeClient->ListenPlatformFeedbackMsg(strdata,nSize);
-}
-
 DataExchangeClient::DataExchangeClient(std::shared_ptr<Channel> channel)
 {
-    gDataExchangeClient = this;
-
     stub_ = DataExchange::NewStub(channel);
 
     dec_yaml(stryamlpath);
-    shmPicFront.mpa = iv::modulecomm::RegisterRecv(shmPicFront.mstrmsgname,ListenFrontData);
-    shmPicRear.mpa = iv::modulecomm::RegisterRecv(shmPicRear.mstrmsgname,ListenRearData);
-    shmPicLeft.mpa = iv::modulecomm::RegisterRecv(shmPicLeft.mstrmsgname,ListenLeftData);
-    shmPicRight.mpa = iv::modulecomm::RegisterRecv(shmPicRight.mstrmsgname,ListenRightData);
-    shmChassis.mpa = iv::modulecomm::RegisterRecv(shmChassis.mstrmsgname,ListenChassisData);
-    shmGPSIMU.mpa = iv::modulecomm::RegisterRecv(shmGPSIMU.mstrmsgname,ListenGPSIMUData);
-    shmPlatformFeedback.mpa = iv::modulecomm::RegisterRecv(shmPlatformFeedback.mstrmsgname,ListenPlatformFeedbackData);
+
+    ModuleFun funupdate = std::bind(&DataExchangeClient::ListenFrontPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmPicFront.mpa = iv::modulecomm::RegisterRecvPlus(shmPicFront.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenRearPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmPicRear.mpa = iv::modulecomm::RegisterRecvPlus(shmPicRear.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenLeftPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmPicLeft.mpa = iv::modulecomm::RegisterRecvPlus(shmPicLeft.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenRightPicMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmPicRight.mpa = iv::modulecomm::RegisterRecvPlus(shmPicRight.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenChassisMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmChassis.mpa = iv::modulecomm::RegisterRecvPlus(shmChassis.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenGPSIMUMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmGPSIMU.mpa = iv::modulecomm::RegisterRecvPlus(shmGPSIMU.mstrmsgname,funupdate);
+    funupdate = std::bind(&DataExchangeClient::ListenPlatformFeedbackMsg,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    shmPlatformFeedback.mpa = iv::modulecomm::RegisterRecvPlus(shmPlatformFeedback.mstrmsgname,funupdate);
 }
 
 DataExchangeClient::~DataExchangeClient(void)
@@ -245,7 +215,7 @@ void DataExchangeClient::dec_yaml(const char *stryamlpath)
     return;
 }
 
-void DataExchangeClient::ListenFrontPicMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenFrontPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::vision::rawpic xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -260,7 +230,7 @@ void DataExchangeClient::ListenFrontPicMsg(const char * strdata,const unsigned i
     gMutex_ImageFront.unlock();
 }
 
-void DataExchangeClient::ListenRearPicMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenRearPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::vision::rawpic xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -275,7 +245,7 @@ void DataExchangeClient::ListenRearPicMsg(const char * strdata,const unsigned in
     gMutex_ImageRear.unlock();
 }
 
-void DataExchangeClient::ListenLeftPicMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenLeftPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::vision::rawpic xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -290,7 +260,7 @@ void DataExchangeClient::ListenLeftPicMsg(const char * strdata,const unsigned in
     gMutex_ImageLeft.unlock();
 }
 
-void DataExchangeClient::ListenRightPicMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenRightPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::vision::rawpic xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -305,7 +275,7 @@ void DataExchangeClient::ListenRightPicMsg(const char * strdata,const unsigned i
     gMutex_ImageRight.unlock();
 }
 
-void DataExchangeClient::ListenChassisMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::chassis xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -319,7 +289,7 @@ void DataExchangeClient::ListenChassisMsg(const char * strdata,const unsigned in
     gMutex_Chassis.unlock();
 }
 
-void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::gps::gpsimu xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -340,9 +310,10 @@ void DataExchangeClient::ListenGPSIMUMsg(const char * strdata,const unsigned int
     heading = xdata.heading();
     speed = GPS_Speed;
     gMutex_GPSIMU.unlock();
+//    std::cout<<heading<<std::endl;
 }
 
-void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const unsigned int nSize) // need a lock
+void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname) // need a lock
 {
     iv::platformFeedback xdata;
     if(!xdata.ParseFromArray(strdata,nSize))
@@ -379,16 +350,20 @@ void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const un
 
     switch (xdata.shift()) {
     case 0: //0N 1D 2R 3P
-        shiftFeedback = ShiftStatus::SHIFT_NEUTRAL;
+        shiftFeedback = ShiftStatus::SHIFT_NEUTRAL; //3P 4R 5N 6D
+        gShift_Status = 5;
         break;
     case 1: //0N 1D 2R 3P
         shiftFeedback = ShiftStatus::SHIFT_DRIVE;
+        gShift_Status = 6;
         break;
     case 2: //0N 1D 2R 3P
         shiftFeedback = ShiftStatus::SHIFT_REVERSE;
+        gShift_Status = 4;
         break;
     case 3: //0N 1D 2R 3P
         shiftFeedback = ShiftStatus::SHIFT_PARKING;
+        gShift_Status = 3;
         break;
     default:
         break;
@@ -396,8 +371,9 @@ void DataExchangeClient::ListenPlatformFeedbackMsg(const char * strdata,const un
 
     throttleFeedback = xdata.throttle();
     brakeFeedback = xdata.brake();
-    steeringWheelAngleFeedback = xdata.steeringwheelangle();
+    steeringWheelAngleFeedback = (-1) * xdata.steeringwheelangle();
     gMutex_PlatformFeedback.unlock();
+//    std::cout<<throttleFeedback<<","<<brakeFeedback<<","<<steeringWheelAngleFeedback<<","<<shiftFeedback<<","<<modeFeedback<<std::endl;
 }
 
 std::string DataExchangeClient::uploadVehicleInfo(void)
@@ -646,6 +622,7 @@ void DataExchangeClient::run()
             updateData(xTime.elapsed() - lastTime);
             std::string reply = uploadVehicleInfo();
             std::cout<< reply <<std::endl;
+            std::cout<<std::setprecision(8)<<destinationPosition.latitude()<<","<<destinationPosition.longitude()<<std::endl;
             lastTime = xTime.elapsed();
         }
 
@@ -666,7 +643,7 @@ void DataExchangeClient::run()
 void DataExchangeClient::patrolPOI_Recieved_Slot(std::string pathID)
 {
     std::cout<<"patrol path calculating"<<std::endl;
-    QFile mapfile("/home/samuel/Documents/path1.txt");
+    QFile mapfile("/home/nvidia/Documents/path2.txt");
     QVector<org::jeecg::defsDetails::grpc::MapPoint> somePoints;
     if(mapfile.open(QIODevice::ReadOnly | QIODevice::Text))
     {

+ 7 - 7
src/driver/driver_cloud_grpc_client_BS/vehicle_upload.h

@@ -65,13 +65,13 @@ public:
 
     void dec_yaml(const char * stryamlpath);
 
-    void ListenFrontPicMsg(const char * strdata,const unsigned int nSize);
-    void ListenRearPicMsg(const char * strdata,const unsigned int nSize);
-    void ListenLeftPicMsg(const char * strdata,const unsigned int nSize);
-    void ListenRightPicMsg(const char * strdata,const unsigned int nSize);
-    void ListenChassisMsg(const char * strdata,const unsigned int nSize);
-    void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize);
-    void ListenPlatformFeedbackMsg(const char * strdata,const unsigned int nSize);
+    void ListenFrontPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenRearPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenLeftPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenRightPicMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenChassisMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenGPSIMUMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+    void ListenPlatformFeedbackMsg(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 protected:
     void run();