change driver/driver_map_xodrload. add extendmap option for extend or not extend.

src/common/modulecommplus/modulecommplus.pro

@@ -17,11 +17,13 @@ DEFINES += QT_DEPRECATED_WARNINGS
 #DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
 
 SOURCES += \
-    modulecommplus.cpp
+    modulecommplus.cpp \
+    modulecommplus_impl.cpp
 
 HEADERS += \
     modulecommplus_global.h \
-    modulecommplus.h
+    modulecommplus.h \
+    modulecommplus_impl.h
 
 # Default rules for deployment.
 unix {

src/common/modulecommplus/modulecommplus_impl.cpp

@@ -1,6 +1,102 @@
 #include "modulecommplus_impl.h"
 
-modulecommplus_impl::modulecommplus_impl()
+#ifdef _WIN32
+
+#include <process.h>
+
+#else
+
+#include <unistd.h>
+
+#endif
+
+#include <thread>
+#include <iostream>
+
+modulecommplus_impl::modulecommplus_impl(char * strmemname)
+{
+    char strsysmemname[256];
+    strncpy(strsysmemname,mstrsysmemname,256);
+    mpSys = new QSharedMemory(strsysmemname);
+
+    if(false == mpSys->attach())
+    {
+        mpSys->create(mnMemMaxCount * sizeof(iv::modulecommplusSendState));
+        iv::modulecommplusSendState * pmodulestate = (iv::modulecommplusSendState *)mpSys->data();
+        int i;
+        for(i=0;i<mnMemMaxCount;i++)
+        {
+            pmodulestate[i].mbUse = false;
+            pmodulestate[i].mnLastInterReq = 0;
+            pmodulestate[i].mnLastRTPSReq = 0;
+            pmodulestate[i].mnLastShareReq = 0;
+            pmodulestate[i].mnWriteCount = 0;
+            pmodulestate[i].mnPID = getpid();
+        }
+    }
+
+    int index = FindAdress(strmemname);
+
+    if(index == -1)
+    {
+RECREATE:
+        index = CreateAdress(strmemname);
+        if(index == -1)
+        {
+            std::cout<<"modulecommplus_impl Fail to Create Adress. Please change mnMemMaxCount"<<std::endl;
+            return;
+        }
+
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        int checkindex = FindAdress(strmemname);
+        if(checkindex != index)
+        {
+            std::cout<<"Becasue Create Time Have 2 Process Change This Adress"<<std::endl;
+            goto RECREATE;
+        }
+
+    }
+
+
+}
+
+int modulecommplus_impl::FindAdress(const char * strmemname)
 {
+    iv::modulecommplusSendState * pmodulestate = (iv::modulecommplusSendState *)mpSys->data();
+
+    int i;
+    int index = -1;
+    for(i = 0;i<mnMemMaxCount;i++)
+    {
+        if(pmodulestate[i].mbUse == true)
+        {
+            if(strncmp(pmodulestate[i].mstrmemename,strmemname,256) == 0)
+            {
+                index = i;
+                break;
+            }
+        }
+    }
+    return index;
+}
+
+int modulecommplus_impl::CreateAdress(const char * strmemname)
+{
+    iv::modulecommplusSendState * pmodulestate = (iv::modulecommplusSendState *)mpSys->data();
+
+    int i;
+    int index = -1;
+
+    for(i=0;i<mnMemMaxCount;i++)
+    {
+        if(pmodulestate[i].mbUse == false)
+        {
+            pmodulestate[i].mbUse = true;
+            strncpy(pmodulestate[i].mstrmemename,strmemname,256);
+            index = i;
+            break;
+        }
+    }
 
+    return index;
 }

src/common/modulecommplus/modulecommplus_impl.h

@@ -2,10 +2,45 @@
 #define MODULECOMMPLUS_IMPL_H
 
 
+#include <QSharedMemory>
+
+namespace  iv {
+
+
+struct modulecommplusSendState
+{
+public:
+    bool mbUse = false; //Default is 0,if 1 this unit is exist
+    char mstrmemename[256];
+    char mstrmemsharename[256];
+    char mstrip[100];
+    int mnPID;
+    qint64 mnWriteCount = 0;
+    qint64 mnLastShareReq = 0;
+    qint64 mnLastInterReq = 0;
+    qint64 mnLastRTPSReq = 0;
+};
+
+}
+
 class modulecommplus_impl
 {
 public:
-    modulecommplus_impl();
+    modulecommplus_impl(char * strmemname);
+
+private:
+    //QSharedMemory Sys For Get state
+    QSharedMemory * mpSys;
+    const int mnMemMaxCount = 1000;
+    const char * mstrsysmemname = "modulecommplusSendState";
+
+    int mnIndex = -1;
+
+private:
+    int FindAdress(const char * strmemname);
+    int CreateAdress(const char * strmemname);
+
+
 };
 
 #endif // MODULECOMMPLUS_IMPL_H

src/driver/driver_map_xodrload/main.cpp

@@ -37,6 +37,8 @@ double glat0,glon0,ghead0;
 
 double gvehiclewidth = 2.0;
 
+bool gbExtendMap = true;
+
 void * gpa;
 void * gpasrc;
 void * gpmap;
@@ -493,7 +495,8 @@ void SetPlan(xodrobj xo)
         pp.x = pp.x + fdis * cos(pp.hdg);
         pp.y = pp.y + fdis * sin(pp.hdg);
         pp.nSignal = 23;
-//        xPlan.push_back(pp);
+        if(gbExtendMap)
+            xPlan.push_back(pp);
 
     }
 
@@ -835,7 +838,7 @@ int main(int argc, char *argv[])
         strmapth = getenv("HOME");
         strmapth = strmapth + "/map/map.xodr";
 //        strmapth = "/home/yuchuli/1226.xodr";
-        strparapath = "./ADCIntelligentVehicle-xodrload.xml";
+        strparapath = "./driver_map_xodrload.xml";
     }
     else
     {
@@ -861,6 +864,8 @@ int main(int argc, char *argv[])
 
     std::string strvehiclewidth = xp.GetParam("vehiclewidth","2.0");
 
+    std::string strextendmap = xp.GetParam("extendmap","false");
+
 
     glat0 = atof(strlat0.data());
     glon0 = atof(strlon0.data());
@@ -868,6 +873,15 @@ int main(int argc, char *argv[])
 
     gvehiclewidth = atof(strvehiclewidth.data());
 
+    if(strextendmap == "true")
+    {
+        gbExtendMap = true;
+    }
+    else
+    {
+        gbExtendMap = false;
+    }
+
 
     LoadXODR(strmapth);
 

src/tool/controller_torquebrake_get_plus/mainwindow.cpp

@@ -189,7 +189,7 @@ void MainWindow::UpdatePlainText(iv::gps::gpsimu &xgpsimu)
 
             }
             fAvgAcc = fAvgAcc/mfVectorAcc.size();
-            if((mfAvgSpeed > 1.0)&&(mbEnable))
+            if((mfAvgSpeed > 1.0)&&(mbEnable) &&(mnTestStep > 0))
             {
                 if(ui->checkBox_notrecordbrake->isChecked() &&(mfSendBrake>0.00001))
                 {

src/tool/map_lanetoxodr/dialogparkingspacefromrtk.cpp

@@ -96,24 +96,24 @@ void DialogParkingSpaceFromRTK::on_pushButton_LoadData_clicked()
     mvectorrtkdata = xvectorrtkdata;
 
     unsigned int nsize = mvectorrtkdata.size();
-    ui->comboBox_Center_From->clear();
-    ui->comboBox_Center_To->clear();
-    ui->comboBox_Heading_From->clear();
-    ui->comboBox_Heading_To->clear();
+    ui->comboBox_LeftUp->clear();
+    ui->comboBox_LeftDown->clear();
+    ui->comboBox_RightUp->clear();
+    ui->comboBox_RightDown->clear();
     if(nsize >= 3)
     {
         unsigned int i;
         for(i=0;i<nsize;i++)
         {
-            ui->comboBox_Heading_From->addItem(QString::number(i));
-            ui->comboBox_Heading_To->addItem(QString::number(i));
-            ui->comboBox_Center_From->addItem(QString::number(i));
-            ui->comboBox_Center_To->addItem(QString::number(i));
+            ui->comboBox_LeftUp->addItem(QString::number(i));
+            ui->comboBox_LeftDown->addItem(QString::number(i));
+            ui->comboBox_RightUp->addItem(QString::number(i));
+            ui->comboBox_RightDown->addItem(QString::number(i));
         }
-        ui->comboBox_Heading_From->setCurrentIndex(1);
-        ui->comboBox_Heading_To->setCurrentIndex(0);
-        ui->comboBox_Center_From->setCurrentIndex(0);
-        ui->comboBox_Center_To->setCurrentIndex(2);
+        ui->comboBox_LeftUp->setCurrentIndex(0);
+        ui->comboBox_LeftDown->setCurrentIndex(1);
+        ui->comboBox_RightUp->setCurrentIndex(3);
+        ui->comboBox_RightDown->setCurrentIndex(2);
     }
 
 
@@ -121,28 +121,79 @@ void DialogParkingSpaceFromRTK::on_pushButton_LoadData_clicked()
 
 void DialogParkingSpaceFromRTK::on_pushButton_Create_clicked()
 {
+    Road * pRoad = mpRoad;
     if(mvectorrtkdata.size()<3)
     {
         QMessageBox::warning(this,"Warning","At Least 3 Points.",QMessageBox::YesAll);
         return;
     }
 
-    int nHeadingFrom = ui->comboBox_Heading_From->currentIndex();
-    int nHeadingTo = ui->comboBox_Heading_To->currentIndex();
-    int nCenterFrom = ui->comboBox_Center_From->currentIndex();
-    int nCenterTo = ui->comboBox_Center_To->currentIndex();
+    int nLeftDown = ui->comboBox_LeftDown->currentIndex();
+    int nLeftUp = ui->comboBox_LeftUp->currentIndex();
+    int nRightUp = ui->comboBox_RightUp->currentIndex();
+    int nRightDown = ui->comboBox_RightDown->currentIndex();
 
-    double fHdg = xodrfunc::CalcHdg(QPointF(mvectorrtkdata[nHeadingFrom].mfrelx,mvectorrtkdata[nHeadingFrom].mfrely),
-                                    QPointF(mvectorrtkdata[nHeadingTo].mfrelx,mvectorrtkdata[nHeadingTo].mfrely));
+    double fHdg = xodrfunc::CalcHdg(QPointF(mvectorrtkdata[nLeftDown].mfrelx,mvectorrtkdata[nLeftDown].mfrely),
+                                    QPointF(mvectorrtkdata[nLeftUp].mfrelx,mvectorrtkdata[nLeftUp].mfrely));
 
     double fCenter_x,fCenter_y;
-    fCenter_x = (mvectorrtkdata[nCenterFrom].mfrelx + mvectorrtkdata[nCenterTo].mfrelx)/2.0;
-    fCenter_y = (mvectorrtkdata[nCenterFrom].mfrely + mvectorrtkdata[nCenterTo].mfrely)/2.0;
+    fCenter_x = (mvectorrtkdata[nLeftUp].mfrelx + mvectorrtkdata[nRightDown].mfrelx)/2.0;
+    fCenter_y = (mvectorrtkdata[nLeftUp].mfrely + mvectorrtkdata[nRightDown].mfrely)/2.0;
+
+    double fLen = sqrt(pow(mvectorrtkdata[nLeftUp].mfrelx - mvectorrtkdata[nLeftDown].mfrelx,2)
+                       +pow(mvectorrtkdata[nLeftUp].mfrely - mvectorrtkdata[nLeftDown].mfrely,2));
+    double fWidth = sqrt(pow(mvectorrtkdata[nLeftDown].mfrelx - mvectorrtkdata[nRightDown].mfrelx ,2)
+                         +pow(mvectorrtkdata[nLeftDown].mfrely - mvectorrtkdata[nRightDown].mfrely,2));
+
+    double fnears = 0;
+    double froadx,froady,froadhdg;
+    pRoad->GetGeometryCoords(0,froadx,froady,froadhdg);
+    double fx,fy,fh,fs;
+    fx = froadx;
+    fy = froady;
+    fh = froadhdg;
+    fs = 0;
+    double fneart = sqrt(pow(froadx - fCenter_x,2) + pow(froady - fCenter_y,2));
+    double s = 0;
+    double sstep = 0.1;
+    s = s+ sstep;
+    while(s<pRoad->GetRoadLength())
+    {
+        if(pRoad->GetGeometryCoords(s,froadx,froady,froadhdg) >= 0)
+        {
+            double fdis = sqrt(pow(froadx - fCenter_x,2) + pow(froady - fCenter_y,2));
+            if(fdis<fneart)
+            {
+                fneart = fdis;
+                fx = froadx;
+                fy = froady;
+                fh = froadhdg;
+                fs = s;
+            }
+        }
+        else
+        {
+            std::cout<<"DialogParkingSpaceFromRTK::on_pushButton_Create_clicked"<<" GetGeometryCoords error. s "
+                    <<s<<" Road id:"<<pRoad->GetRoadId()<<" len:"<<pRoad->GetRoadLength()<<std::endl;
+        }
+        s= s+sstep;
+    }
 
-    double fLen = sqrt(pow(mvectorrtkdata[nHeadingFrom].mfrelx - mvectorrtkdata[nHeadingTo].mfrelx,2)
-                       +pow(mvectorrtkdata[nHeadingFrom].mfrely - mvectorrtkdata[nHeadingTo].mfrely,2));
-    double fWidth = sqrt(pow(mvectorrtkdata[nHeadingFrom].mfrelx - mvectorrtkdata[nCenterTo].mfrelx ,2)
-                         +pow(mvectorrtkdata[nHeadingFrom].mfrely - mvectorrtkdata[nCenterTo].mfrely,2));
+    if(fneart > 0)
+    {
+        double fhdg2 = xodrfunc::CalcHdg(QPointF(fx,fy),QPointF(fCenter_x,fCenter_y));
+        double fhdgdiff = fhdg2 - fh;
+        while(fhdgdiff<0)fhdgdiff = fhdgdiff + 2.0*M_PI;
+        while(fhdgdiff >= 2.0*M_PI)fhdgdiff = fhdgdiff - 2.0*M_PI;
+        if(fhdgdiff > M_PI)fneart = fneart * (-1);
+
+    }
+    pRoad->AddObject("0",fs,fneart,0);
+    Object * pObj = pRoad->GetLastAddedObject();
+    pObj->Settype("parkingSpace");
+    pObj->Sethdg(fHdg);
+    pObj->Setlength(fLen);
+    pObj->Setwidth(fWidth);
 
     qDebug("fhdg:%f flen:%f fwidth:%f",fHdg,fLen,fWidth);
 }

src/tool/map_lanetoxodr/dialogparkingspacefromrtk.ui

@@ -41,12 +41,12 @@
     <rect>
      <x>60</x>
      <y>263</y>
-     <width>151</width>
+     <width>101</width>
      <height>31</height>
     </rect>
    </property>
    <property name="text">
-    <string>Heading               From</string>
+    <string>左上：</string>
    </property>
   </widget>
   <widget class="QLabel" name="label_2">
@@ -59,20 +59,20 @@
     </rect>
    </property>
    <property name="text">
-    <string>To</string>
+    <string>右上</string>
    </property>
   </widget>
-  <widget class="QComboBox" name="comboBox_Heading_From">
+  <widget class="QComboBox" name="comboBox_LeftUp">
    <property name="geometry">
     <rect>
-     <x>240</x>
-     <y>263</y>
+     <x>190</x>
+     <y>260</y>
      <width>71</width>
      <height>31</height>
     </rect>
    </property>
   </widget>
-  <widget class="QComboBox" name="comboBox_Heading_To">
+  <widget class="QComboBox" name="comboBox_RightUp">
    <property name="geometry">
     <rect>
      <x>410</x>
@@ -87,15 +87,15 @@
     <rect>
      <x>60</x>
      <y>314</y>
-     <width>151</width>
+     <width>81</width>
      <height>31</height>
     </rect>
    </property>
    <property name="text">
-    <string>Center Point      From</string>
+    <string>左下</string>
    </property>
   </widget>
-  <widget class="QComboBox" name="comboBox_Center_To">
+  <widget class="QComboBox" name="comboBox_RightDown">
    <property name="geometry">
     <rect>
      <x>410</x>
@@ -105,11 +105,11 @@
     </rect>
    </property>
   </widget>
-  <widget class="QComboBox" name="comboBox_Center_From">
+  <widget class="QComboBox" name="comboBox_LeftDown">
    <property name="geometry">
     <rect>
-     <x>240</x>
-     <y>314</y>
+     <x>190</x>
+     <y>310</y>
      <width>71</width>
      <height>31</height>
     </rect>
@@ -125,7 +125,7 @@
     </rect>
    </property>
    <property name="text">
-    <string>To</string>
+    <string>右下</string>
    </property>
   </widget>
   <widget class="QPushButton" name="pushButton_Create">
@@ -151,7 +151,7 @@
     </rect>
    </property>
    <property name="text">
-    <string>RTK顺序默认为左上、左下、右下、右上，否则调整From和To</string>
+    <string>RTK顺序默认为左上、左下、右下、右上，否则调整</string>
    </property>
   </widget>
  </widget>