change pythondecisiondemo for add object.

src/python/pythondecisiondemo/decisiondemo.py

@@ -4,6 +4,8 @@ import proto.gpsimu_pb2 as gpsimu_pb2
 import proto.mapdata_pb2 as mapdata_pb2
 import proto.decition_pb2 as decition_pb2
 
+from proto import objectarray_pb2 as objectarray_pb2
+
 import math
 from typing import List  
 
@@ -28,18 +30,21 @@ class DecisionDemo:
         self.mspeed = 10.0  #运行速度10km/h
         pass
 
-    def CalcDecision(self,tracemap : mapdata_pb2.tracemap, msg_gpsimu : gpsimu_pb2.gpsimu):
+    def CalcDecision(self,tracemap : mapdata_pb2.tracemap, msg_gpsimu : gpsimu_pb2.gpsimu,xobjarray : objectarray_pb2.objectarray):
   #      x,y = self.GaussProj(gpsimu_pb2.lon,gpsimu_pb2.lat)
-        print(" in calc: map size: ",len(tracemap.point))
+ #       print(" in calc: map size: ",len(tracemap.point))
         nearindex = self.FindNearIndex(tracemap,msg_gpsimu)
-        print("near index: ",nearindex)
+ #       print("near index: ",nearindex)
+
+        for p in xobjarray.obj :
+            print(f"x: {p.position.x}  y: {p.position.y} ")
 
         if nearindex < 0 :
             print(" not found near point, return")
             return
         
         localpoints,distoend = self.CalcLocalPath(nearindex,tracemap,msg_gpsimu)
-        print(f"disttoend: {distoend}")
+ #       print(f"disttoend: {distoend}")
 
         realspeed = 3.6 * math.sqrt(math.pow(msg_gpsimu.vn,2) + math.pow(msg_gpsimu.ve,2))
 
@@ -54,7 +59,7 @@ class DecisionDemo:
         else :
             endacc = self.mendacc
         
-        print("endacc: ",endacc)
+ #       print("endacc: ",endacc)
         if endacc<-1e-9:
             acc = endacc
 
@@ -130,7 +135,7 @@ class DecisionDemo:
                     acc = 0.0
         
 
-        print("ppindex : ",ppindex)
+#        print("ppindex : ",ppindex)
         denominator = 2 * localpoints[ppindex].mx *(-1);
         numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2);
         fRadius = 1e9;
@@ -205,6 +210,29 @@ class DecisionDemo:
             if distoend > 300:
                 break
         return localpoints,distoend
+    
+    def is_point_in_rotated_rectangle(self,x, y, x1, y1, yaw, l, w):  
+        # 将长方形的左下角坐标转换到原点  
+        x_rel = x - x1  
+        y_rel = y - y1  
+  
+        # 计算旋转矩阵（逆时针旋转）  
+        # | cos(yaw)  -sin(yaw) |  
+        # | sin(yaw)   cos(yaw) |  
+        cos_yaw = math.cos(yaw)  
+        sin_yaw = math.sin(yaw)  
+      
+        # 应用旋转矩阵到相对坐标  
+        x_rotated = x_rel * cos_yaw + y_rel * sin_yaw  
+        y_rotated = -x_rel * sin_yaw + y_rel * cos_yaw  
+  
+        # 判断点是否在旋转后的长方形内  
+        # 长方形的边界在旋转后的坐标系中是 [-l/2, l/2] x [-w/2, w/2]  
+        if -l/2 <= x_rotated <= l/2 and -w/2 <= y_rotated <= w/2:  
+            return True  
+        else:  
+            return False  
+  
 
     def GaussProj(self,lon,lat):
         iPI = 0.0174532925199433

src/python/pythondecisiondemo/proto/objectarray_pb2.py

@@ -11,7 +11,7 @@ from google.protobuf import symbol_database as _symbol_database
 _sym_db = _symbol_database.Default()
 
 
-import object_pb2 as object__pb2
+from . import object_pb2 as object__pb2
 
 
 DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x11objectarray.proto\x12\x08iv.lidar\x1a\x0cobject.proto\"D\n\x0bobjectarray\x12\"\n\x03obj\x18\x01 \x03(\x0b\x32\x15.iv.lidar.lidarobject\x12\x11\n\ttimestamp\x18\x02 \x01(\x01')

src/python/pythondecisiondemo/pythondecisiondemo.py

@@ -1,11 +1,14 @@
 import modulecommpython
 import numpy as np  
 import time
+import sys  
 
 import proto.gpsimu_pb2 as gpsimu_pb2
 #import gpsimu_pb2
 import proto.mapdata_pb2 as mapdata_pb2
 import proto.decition_pb2 as decition_pb2
+import proto.object_pb2 as object_pb2
+from proto import objectarray_pb2 as objectarray_pb2
 
 from PyModuleCommModule import PyModuleComm
 from decisiondemo import DecisionDemo
@@ -16,11 +19,15 @@ msg_gpsimu = gpsimu_pb2.gpsimu()
 time_mapdata = 0
 msg_mapdata = mapdata_pb2.tracemap()
 
+time_objectarray = 0
+msg_objectarray = objectarray_pb2.objectarray()
+
 gdecision = DecisionDemo()
 
 
 def gpsimu_callback(arr : np,nsize,time):  
     global msg_gpsimu
+    global time_gpsimu
  #   print("Python callback function called from C++!. time: ",time)
  #   print("    size: ",nsize)  
     sub_arr = arr[0:nsize]
@@ -34,6 +41,7 @@ def gpsimu_callback(arr : np,nsize,time):
 
 def mapdata_callback(arr : np,nsize,time):   
     global msg_mapdata
+    global time_mapdata
     sub_arr = arr[0:nsize]
     databytes = sub_arr.tobytes() #sub_arr.tostring()
     msg = mapdata_pb2.tracemap()
@@ -42,18 +50,31 @@ def mapdata_callback(arr : np,nsize,time):
     time_mapdata = time
     length = len(msg_mapdata.point)
     print("map point lenth: ",length)
+
+def objectarray_callback(arr : np,nsize,time):
+    global msg_objectarray
+    global time_objectarray
+    sub_arr = arr[0:nsize]
+    databytes = sub_arr.tobytes() #sub_arr.tostring()
+    msg = objectarray_pb2.objectarray()
+    msg.ParseFromString(databytes)
+    msg_objectarray = msg
+    time_objectarray = time
+ #   length = len(msg_objectarray.obj)
+ #   print("obj size: ",length)
+    pass
     
 def SendDefDecision(md : PyModuleComm):
     pass
 
-def MakeDecision(md : PyModuleComm, mapdata : mapdata_pb2.tracemap, gpsimu : gpsimu_pb2.gpsimu):
+def MakeDecision(md : PyModuleComm, mapdata : mapdata_pb2.tracemap, gpsimu : gpsimu_pb2.gpsimu,xobjarray : objectarray_pb2.objectarray):
     global gdecision
     length = len(mapdata.point)
     if length < 1 :
         print(" no tracemap")
         return
-    print("map1 point lenth: ",length)
-    xdecision = gdecision.CalcDecision(mapdata,gpsimu)
+#    print("map1 point lenth: ",length)
+    xdecision = gdecision.CalcDecision(mapdata,gpsimu,xobjarray)
     serialized_str = xdecision.SerializeToString()  
     serialized_array = np.frombuffer(serialized_str, dtype=np.uint8)  
     length = len(serialized_array)
@@ -66,7 +87,6 @@ def main():
 
     # 初始化一个变量  
     count = 0
-
     count = 3.5/1.5
     count = int(count)
     print("count = ",count)  
@@ -77,11 +97,14 @@ def main():
     mmap = PyModuleComm("newtracemap")
     mmap.RegisterRecv(mapdata_callback)
 
+    mobj = PyModuleComm("lidar_track")
+    mobj.RegisterRecv(objectarray_callback)
+
     md = PyModuleComm("deciton")
     md.RegiseterSend(100000,1)
     # 使用while循环，只要count小于10，就继续循环  
     while count < 10:
-        MakeDecision(md,msg_mapdata,msg_gpsimu)
+        MakeDecision(md,msg_mapdata,msg_gpsimu,msg_objectarray)
         time.sleep(0.05)