change pythondecisiondemo, add some ctrl code, not complete.

src/common/common/license_local/adclicense.cpp

@@ -80,7 +80,7 @@ QString ADCLicenseServ::readLicense()
     if (!QFile::exists(filePath)){
         if (file.open(QIODevice::WriteOnly | QIODevice::Text)) {
            QTextStream out(&file);
-           out << " " << endl;
+            out << " "<<endl;
            file.close();
            qDebug() << "File created and written successfully.";
            return "0";

src/common/ivbacktrace/ivbacktrace.cpp

@@ -258,10 +258,14 @@ void out_stack(char *sig)
 
     savetofile(stroutput);
 
-    QTime x;
-    x.start();
-    while((unsigned int)(x.elapsed())<100)
+    int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
+//    QTime x;
+//    x.start();
+    int64_t nnow = std::chrono::steady_clock::now().time_since_epoch().count();
+//    while((unsigned int)(x.elapsed())<100)
+    while(((nnow - nstart)/1e6)<100)
     {
+        nnow = std::chrono::steady_clock::now().time_since_epoch().count();
  //       qDebug("hello");
     }
 

src/driver/driver_map_xodrload/service_roi_xodr.cpp

@@ -1,5 +1,6 @@
 #include "service_roi_xodr.h"
 
+#include <chrono>
 #include <iostream>
 #include "gnss_coordinate_convert.h"
 
@@ -38,8 +39,9 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
     unsigned int j;
     std::vector<iv::roi::area> xvectorarea;
 
-    QTime xTime;
-    xTime.start();
+//    QTime xTime;
+    int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
+ //   xTime.start();
 
     //Find Near ROI area
     for(i=0;i<nroadsize;i++)
@@ -83,7 +85,8 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
         }
     }
 
-    std::cout<<"xTime: "<<xTime.elapsed()<<std::endl;
+    int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    std::cout<<"xTime: "<<(nnow - nstart)/1e6<<std::endl;
 
     if(xreq_ptr->gridsize() <= 0.0)
     {
@@ -162,7 +165,9 @@ int service_roi_xodr::GetROI(std::shared_ptr<iv::roi::request> xreq_ptr, std::sh
     }
 
 
-    std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
+    nnow = std::chrono::system_clock::now().time_since_epoch().count();
+    std::cout<<"xTime: "<<(nnow - nstart)/1e6<<" size : "<<xvectorpoint.size()<< std::endl;
+ //   std::cout<<"xTime: "<<xTime.elapsed()<<" size : "<<xvectorpoint.size()<< std::endl;
 
     for(i=0;i<xvectorpoint.size();i++)
     {

src/python/pythondecisiondemo/PyModuleCommModule.py

@@ -54,7 +54,7 @@ class PyModuleComm:
     def threadrecvdata(self, arg):  
         # 这个函数将被线程执行  
  #       print(f"线程开始执行，参数是 {arg}")  
-        nBuffSize = 1000
+        nBuffSize = int(1000)
         arr = np.zeros(nBuffSize,dtype=np.int8)
         recvtime = np.zeros(1,dtype=np.int64)
         nrealsize = np.zeros(1,dtype=np.int32)
@@ -65,7 +65,7 @@ class PyModuleComm:
             else:
                 pass
             if nrtn < 0:
-                nBuffSize = nrealsize[0] * 2
+                nBuffSize = int(nrealsize[0] * 2)
                 arr = np.zeros(nBuffSize,dtype=np.int8)
             else:
                 time.sleep(0.001)

src/python/pythondecisiondemo/decisiondemo.py

@@ -3,21 +3,151 @@ import gpsimu_pb2
 import mapdata_pb2
 
 import math
+from typing import List  
+
+from datetime import datetime, timedelta  
+import time  
 
 class Point2D:
     def __init__(self, x, y):
         self.mx = x
         self.my = y
 
+    def __str__(self):  
+        return f"Point2D({self.mx}, {self.my})"  
+
 class DecisionDemo:  
     def __init__(self):  
         # 初始化代码...  
         pass
 
-    def CalcDecision(self,msg_mapdata : mapdata_pb2, msg_gpsimu : gpsimu_pb2):
-        x,y = self.GaussProj(gpsimu_pb2.lon,gpsimu_pb2.lat)
+    def CalcDecision(self,tracemap : mapdata_pb2.tracemap, msg_gpsimu : gpsimu_pb2.gpsimu):
+  #      x,y = self.GaussProj(gpsimu_pb2.lon,gpsimu_pb2.lat)
+        print(" in calc: map size: ",len(tracemap.point))
+        nearindex = self.FindNearIndex(tracemap,msg_gpsimu)
+        print("near index: ",nearindex)
 
-        pass
+        if nearindex < 0 :
+            print(" not found near point, return")
+            return
+        
+        localpoints,distoend = self.CalcLocalPath(nearindex,tracemap,msg_gpsimu)
+        print(f"disttoend: {distoend}")
+
+        realspeed = 3.6 * math.sqrt(math.pow(msg_gpsimu.vn,2) + math.pow(msg_gpsimu.ve,2))
+
+    def CalcCmd(self,localpoints : List[Point2D],realspeed):
+        desiredspeed = 10
+        defaultacc = 1.0
+        pd = realspeed * 0.3
+        if pd < 4.0 : 
+            pd = 4.0
+        
+        sumdis = 0
+        i = 1
+        size = len(localpoints)
+        ppindex = 0
+        
+        while i < size:
+            sumdis = math.sqrt(math.pow(localpoints[i].mx - localpoints[i-1].mx,2) 
+                               + math.pow(localpoints[i].my - localpoints[i-1].my,2))
+            ppindex = i
+            if sumdis >= pd :
+                ppindex = i
+                break
+        acc = -0.5
+        wheel = 0.0
+        if ppindex < 3:
+            acc = 0.0
+            wheel = 0.0
+            return acc,wheel
+        
+        if desiredspeed > 0.1:
+            if realspeed < desiredspeed :
+                if realspeed < desiredspeed*0.9:
+                    acc = defaultacc
+                else:
+                    acc = defaultacc *(desiredspeed - realspeed) * (1/0.1)/desiredspeed
+            else:
+                if realspeed > desiredspeed*1.1:
+                    if realspeed > desiredspeed * 2.0:
+                        acc = defaultacc * (-1.0)
+                    else:
+                        acc = defaultacc *  (desiredspeed - realspeed)/desiredspeed
+                else:
+                    acc = 0.0
+        
+
+        denominator = 2 * localpoints[ppindex].mx *(-1);
+        numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2);
+        fRadius = 1e9;
+        if math.fabs(denominator)>0:
+            fRadius = numerator/denominator
+        else:
+            fRadius = 1e9
+        if fRadius == 0:
+            wheel = 0
+        kappa = 1.0/fRadius
+        wheel_base = 2.9
+        wheelratio = 13.6
+        wheel = (-1.0)*kappa * wheel_base * wheelratio * 180.0/math.pi
+
+        return acc,wheel
+        
+
+    def FindNearIndex(self,tracemap : mapdata_pb2.tracemap,xgpsimu : gpsimu_pb2.gpsimu) :
+        x,y = self.GaussProj(xgpsimu.lon,xgpsimu.lat)
+        nearindex = -1
+        neardis = 100000
+        index = 0
+        for p in tracemap.point:
+            dis = math.sqrt(math.pow(x - p.gps_x,2) + math.pow(y - p.gps_y,2))
+            headingdiff = xgpsimu.heading - p.ins_heading_angle
+            while headingdiff < -180:
+                headingdiff = headingdiff + 360
+            while headingdiff >= 180:
+                headingdiff = headingdiff - 360
+            if math.fabs(headingdiff) < 80 :
+                if dis < neardis :
+                    neardis = dis
+                    nearindex = index
+            index = index + 1
+        return nearindex
+    
+    def CalcLocalPath(self,index,xtracemap : mapdata_pb2.tracemap,xgpsimu : gpsimu_pb2.gpsimu):
+        x0,y0 = self.GaussProj(xgpsimu.lon,xgpsimu.lat)
+        localpoints = []
+        i = index
+        npoint = 0;
+        nmaxpoint = 600
+        length = len(xtracemap.point)
+        distoend = 0;
+        while i< length :
+            xraw = xtracemap.point[i].gps_x - x0
+            yraw = xtracemap.point[i].gps_y - y0
+     #       print(f"i: {i-index} xraw:{xraw} yraw:{yraw} ")
+            theta = (90 - xgpsimu.heading) * math.pi /180.0
+            theta = theta * (-1)
+            theta = theta  + math.pi/2.0
+            x = xraw * math.cos(theta) - yraw * math.sin(theta)
+            y = xraw * math.sin(theta) + yraw * math.cos(theta)
+     #       print(f"i: {i-index} x:{x} y:{y} ")
+            localpoints.append(Point2D(x,y))
+            if i>index :
+                distoend = distoend + math.sqrt(math.pow(xtracemap.point[i].gps_x - xtracemap.point[i-1].gps_x,2) 
+                                                + math.pow(xtracemap.point[i].gps_y - xtracemap.point[i-1].gps_y,2))
+            if npoint >= nmaxpoint:
+                break
+            i = i+1
+            npoint = npoint + 1
+        while i < length :
+            if i>index :
+                distoend = distoend + math.sqrt(math.pow(xtracemap.point[i].gps_x - xtracemap.point[i-1].gps_x,2) 
+                                                + math.pow(xtracemap.point[i].gps_y - xtracemap.point[i-1].gps_y,2))
+            i = i+1
+            if distoend > 300:
+                break
+        return localpoints,distoend
 
     def GaussProj(self,lon,lat):
         iPI = 0.0174532925199433

src/python/pythondecisiondemo/pythondecisiondemo.py

@@ -14,29 +14,44 @@ msg_gpsimu = gpsimu_pb2.gpsimu()
 time_mapdata = 0
 msg_mapdata = mapdata_pb2.tracemap()
 
+gdecision = DecisionDemo()
+
 
 def gpsimu_callback(arr : np,nsize,time):  
-    print("Python callback function called from C++!. time: ",time)
-    print("    size: ",nsize)  
+    global msg_gpsimu
+ #   print("Python callback function called from C++!. time: ",time)
+ #   print("    size: ",nsize)  
     sub_arr = arr[0:nsize]
-    databytes = sub_arr.tostring()
+    databytes = sub_arr.tobytes()
     msg = gpsimu_pb2.gpsimu()
     msg.ParseFromString(databytes)
     msg_gpsimu = msg
     time_gpsimu = time
-    print("lon: ",msg.lon)
+ #   print("lon: ",msg.lon)
 
 
 def mapdata_callback(arr : np,nsize,time):   
+    global msg_mapdata
     sub_arr = arr[0:nsize]
-    databytes = sub_arr.tostring()
+    databytes = sub_arr.tobytes() #sub_arr.tostring()
     msg = mapdata_pb2.tracemap()
     msg.ParseFromString(databytes)
     msg_mapdata = msg
     time_mapdata = time
+    length = len(msg_mapdata.point)
+    print("map point lenth: ",length)
     
+def SendDefDecision(md : PyModuleComm):
+    pass
 
-def MakeDecision(md : PyModuleComm, msg_mapdata : mapdata_pb2, msg_gpsimu : gpsimu_pb2):
+def MakeDecision(md : PyModuleComm, mapdata : mapdata_pb2.tracemap, gpsimu : gpsimu_pb2.gpsimu):
+    global gdecision
+    length = len(mapdata.point)
+    if length < 1 :
+        print(" no tracemap")
+        return
+    print("map1 point lenth: ",length)
+    gdecision.CalcDecision(mapdata,gpsimu)
     pass
 
     
@@ -46,14 +61,6 @@ def main():
     # 初始化一个变量  
     count = 0
 
-    ad = DecisionDemo()
-    x,y = ad.GaussProj(39.1,119.1)
-    print("x = ",x)
-    print("y = ",y)
-
-    lon,lat = ad.GaussProjInvCal(x,y)
-    print("lon = ",lon)
-    print("lat = ",lat)
 
     count = 3.5/1.5
     count = int(count)
@@ -64,9 +71,12 @@ def main():
 
     mmap = PyModuleComm("newtracemap")
     mmap.RegisterRecv(mapdata_callback)
- 
+
+    md = PyModuleComm("decision")
+    md.RegiseterSend(100000,1)
     # 使用while循环，只要count小于10，就继续循环  
     while count < 10:
+        MakeDecision(md,msg_mapdata,msg_gpsimu)
         time.sleep(1.0)
 
   

src/tool/tool_xodrobj/mainwindow.cpp

@@ -305,14 +305,14 @@ void MainWindow::AdjustWPos(QSize sizemain)
 void MainWindow::ExecPainter()
 {
 
-    QTime x;
+  //  QTime x;
 
-
-    x.start();
+  //  int64_t nstart = std::chrono::system_clock::now().time_since_epoch().count();
+  //  x.start();
     //    qDebug("painter.");
     
     painter->begin(image);
-    qDebug("time is %d",x.elapsed());
+  //  qDebug("time is %d",x.elapsed());
     image->fill(QColor(255, 255, 255));//对画布进行填充
     
     //    std::vector<iv::GPSData> navigation_data = brain->navigation_data;