change pythondecisondemo, add some code, not complete.

src/common/modulecomm/modulecommpython.cpp

@@ -212,7 +212,7 @@ public:
 
     void threadcallback()
     {
-        mySignal.connect(boost::bind(onSignal, _1));  
+        mySignal.connect(boost::bind(onSignal, std::placeholders::_1));  
         while(mbRun)
         {
             std::this_thread::sleep_for(std::chrono::milliseconds(1000));

src/python/pythondecisiondemo/decisiondemo.py

@@ -0,0 +1,78 @@
+
+import gpsimu_pb2
+import mapdata_pb2
+
+import math
+
+class Point2D:
+    def __init__(self, x, y):
+        self.mx = x
+        self.my = y
+
+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)
+
+        pass
+
+    def GaussProj(self,lon,lat):
+        iPI = 0.0174532925199433
+        ZoneWide = 6
+        a = 6378245.0
+        f = 1.0 / 298.3
+        ProjNo = int(lon / ZoneWide)
+        longitude0 = ProjNo * ZoneWide + ZoneWide / 2
+        longitude0 = longitude0 * iPI
+        latitude0 = 0
+        longitude1 = lon * iPI #经度转换为弧度
+        latitude1 = lat * iPI #//纬度转换为弧度
+        e2 = 2 * f - f * f
+        ee = e2 * (1.0 - e2)
+        NN = a / math.sqrt(1.0 - e2 * math.sin(latitude1)*math.sin(latitude1))
+        T = math.tan(latitude1)*math.tan(latitude1)
+        C = ee * math.cos(latitude1)*math.cos(latitude1)
+        A = (longitude1 - longitude0)*math.cos(latitude1)
+        M = a * ((1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256)*latitude1 - (3 * e2 / 8 + 3 * e2*e2 / 32 + 45 * e2*e2*e2 / 1024)*math.sin(2 * latitude1)+ (15 * e2*e2 / 256 + 45 * e2*e2*e2 / 1024)*math.sin(4 * latitude1) - (35 * e2*e2*e2 / 3072)*math.sin(6 * latitude1))
+        xval = NN * (A + (1 - T + C)*A*A*A / 6 + (5 - 18 * T + T * T + 72 * C - 58 * ee)*A*A*A*A*A / 120)
+        yval = M + NN * math.tan(latitude1)*(A*A / 2 + (5 - T + 9 * C + 4 * C*C)*A*A*A*A / 24 + (61 - 58 * T + T * T + 600 * C - 330 * ee)*A*A*A*A*A*A / 720)
+        X0 = 1000000 * (ProjNo + 1) + 500000
+        Y0 = 0
+        xval = xval + X0; yval = yval + Y0;
+        X = xval
+        Y = yval
+        return X,Y
+    
+    def GaussProjInvCal(self,X,Y):
+        iPI = 0.0174532925199433   #3.1415926535898/180.0;
+        a = 6378245.0
+        f = 1.0 / 298.3 # //54年北京坐标系参数
+                #////a=6378140.0; f=1/298.257; //80年西安坐标系参数
+        ZoneWide = 6  # ////6度带宽
+        ProjNo = int(X / 1000000)  # //查找带号
+        longitude0 = (ProjNo - 1) * ZoneWide + ZoneWide / 2
+        longitude0 = longitude0 * iPI  # //中央经线
+        X0 = ProjNo * 1000000 + 500000  
+        Y0 = 0
+        xval = X - X0; yval = Y - Y0 #//带内大地坐标
+        e2 = 2 * f - f * f 
+        e1 = (1.0 - math.sqrt(1 - e2)) / (1.0 + math.sqrt(1 - e2))
+        ee = e2 / (1 - e2)
+        M = yval
+        u = M / (a*(1 - e2 / 4 - 3 * e2*e2 / 64 - 5 * e2*e2*e2 / 256))
+        fai = u + (3 * e1 / 2 - 27 * e1*e1*e1 / 32)*math.sin(2 * u) + (21 * e1*e1 / 16 - 55 * e1*e1*e1*e1 / 32)*math.sin(4 * u)+ (151 * e1*e1*e1 / 96)*math.sin(6 * u) + (1097 * e1*e1*e1*e1 / 512)*math.sin(8 * u)
+        C = ee * math.cos(fai)*math.cos(fai)
+        T = math.tan(fai)*math.tan(fai)
+        NN = a / math.sqrt(1.0 - e2 * math.sin(fai)*math.sin(fai))
+        R = a * (1 - e2) / math.sqrt((1 - e2 * math.sin(fai)*math.sin(fai))*(1 - e2 * math.sin(fai)*math.sin(fai))*(1 - e2 * math.sin(fai)*math.sin(fai)))
+        D = xval / NN
+        #//计算经度(Longitude) 纬度(Latitude)
+        longitude1 = longitude0 + (D - (1 + 2 * T + C)*D*D*D / 6 + (5 - 2 * C + 28 * T - 3 * C*C + 8 * ee + 24 * T*T)*D*D*D*D*D / 120) / math.cos(fai)
+        latitude1 = fai - (NN*math.tan(fai) / R)*(D*D / 2 - (5 + 3 * T + 10 * C - 4 * C*C - 9 * ee)*D*D*D*D / 24 + (61 + 90 * T + 298 * C + 45 * T*T - 256 * ee - 3 * C*C)*D*D*D*D*D*D / 720)
+        #//转换为度 DD
+        longitude = longitude1 / iPI
+        latitude = latitude1 / iPI
+        return longitude,latitude

src/python/pythondecisiondemo/pythondecisiondemo.py

@@ -6,6 +6,7 @@ import gpsimu_pb2
 import mapdata_pb2
 
 from PyModuleCommModule import PyModuleComm
+from decisiondemo import DecisionDemo
 
 time_gpsimu = 0
 msg_gpsimu = gpsimu_pb2.gpsimu()
@@ -35,7 +36,7 @@ def mapdata_callback(arr : np,nsize,time):
     time_mapdata = time
     
 
-def MakeDecision(md : PyModuleComm, msg_mapdata : mapdata_pb2, msg_gpsimu : gpsimu_pb2)
+def MakeDecision(md : PyModuleComm, msg_mapdata : mapdata_pb2, msg_gpsimu : gpsimu_pb2):
     pass
 
     
@@ -43,7 +44,20 @@ def MakeDecision(md : PyModuleComm, msg_mapdata : mapdata_pb2, msg_gpsimu : gpsi
 def main():  
 
     # 初始化一个变量  
-    count = 0  
+    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)
+    print("count = ",count)  
 
     mc = PyModuleComm("hcp2_gpsimu")
     mc.RegisterRecv(gpsimu_callback)

src/python/pythondecisiondemo/readme.md

@@ -0,0 +1,3 @@
+sudo apt install python3-numpy
+
+sudo apt install python3-protobuf