change pythondecisiondemo. test control ok.

src/python/pythondecisiondemo/PyModuleCommModule.py

@@ -48,8 +48,8 @@ class PyModuleComm:
         self.obj.RegisterSend(self.strmemname,nSize,nPacCount)
     
     def SendData(self,arr,nsendsize):      
-        nrealsize = np.zeros(1, dtype=np.int32)  
-        nrtn = self.obj.SendData(arr,nsendsize,nrealsize)
+    #    nrealsize = np.zeros(1, dtype=np.int32)  
+        nrtn = self.obj.SendData(arr,nsendsize)
   
     def threadrecvdata(self, arg):  
         # 这个函数将被线程执行  

src/python/pythondecisiondemo/decisiondemo.py

@@ -1,6 +1,8 @@
 
-import gpsimu_pb2
-import mapdata_pb2
+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 math
 from typing import List  
@@ -12,6 +14,7 @@ class Point2D:
     def __init__(self, x, y):
         self.mx = x
         self.my = y
+        
 
     def __str__(self):  
         return f"Point2D({self.mx}, {self.my})"  
@@ -19,6 +22,10 @@ class Point2D:
 class DecisionDemo:  
     def __init__(self):  
         # 初始化代码...  
+        self.mendacc = -0.7  #抵达终点时的舰速度为-0.7
+        self.mmaxwheel = 430 #最大方向盘角度
+        self.mdefaultacc = 1.0  #加速时的acc
+        self.mspeed = 10.0  #运行速度10km/h
         pass
 
     def CalcDecision(self,tracemap : mapdata_pb2.tracemap, msg_gpsimu : gpsimu_pb2.gpsimu):
@@ -36,9 +43,53 @@ class DecisionDemo:
 
         realspeed = 3.6 * math.sqrt(math.pow(msg_gpsimu.vn,2) + math.pow(msg_gpsimu.ve,2))
 
+        acc,wheel,speed = self.CalcCmd(localpoints,realspeed)
+
+        #calc end acc, stop to end point
+        endacc = 0.0
+        if distoend > 0.1:
+            if distoend < math.pow(realspeed/3.6,2)/(2*math.fabs(self.mendacc)) :
+                endacc = math.pow(realspeed/3.6,2)/(2*distoend) *(-1.0)
+                print("endacc: ",endacc)
+        else :
+            endacc = self.mendacc
+        
+        print("endacc: ",endacc)
+        if endacc<-1e-9:
+            acc = endacc
+
+        if acc < -5.0:
+            acc = -5.0
+
+
+        xdecisiion = decition_pb2.decition()
+        xdecisiion.wheelAngle = wheel
+        xdecisiion.accelerator = acc
+        xdecisiion.brake = 0
+        xdecisiion.speed = speed
+        print("acc = : ",acc)
+        if acc < 0:
+            xdecisiion.brake = acc
+            xdecisiion.torque = 0
+        else:
+            xdecisiion.brake = 0
+            fVehWeight = 1800
+ #           fg = 9.8
+            fRollForce = 50
+            fRatio = 2.5
+            fNeed = fRollForce + fVehWeight*acc
+            xdecisiion.torque = fNeed/fRatio
+
+        return xdecisiion
+
+        
+
+    
+
+
     def CalcCmd(self,localpoints : List[Point2D],realspeed):
-        desiredspeed = 10
-        defaultacc = 1.0
+        desiredspeed = self.mspeed
+        defaultacc = self.mdefaultacc
         pd = realspeed * 0.3
         if pd < 4.0 : 
             pd = 4.0
@@ -49,18 +100,19 @@ class DecisionDemo:
         ppindex = 0
         
         while i < size:
-            sumdis = math.sqrt(math.pow(localpoints[i].mx - localpoints[i-1].mx,2) 
+            sumdis = 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
+            i = i+1
         acc = -0.5
         wheel = 0.0
         if ppindex < 3:
             acc = 0.0
             wheel = 0.0
-            return acc,wheel
+            return acc,wheel,0
         
         if desiredspeed > 0.1:
             if realspeed < desiredspeed :
@@ -78,6 +130,7 @@ class DecisionDemo:
                     acc = 0.0
         
 
+        print("ppindex : ",ppindex)
         denominator = 2 * localpoints[ppindex].mx *(-1);
         numerator = math.pow(localpoints[ppindex].mx,2) + pow(localpoints[ppindex].my,2);
         fRadius = 1e9;
@@ -90,9 +143,13 @@ class DecisionDemo:
         kappa = 1.0/fRadius
         wheel_base = 2.9
         wheelratio = 13.6
-        wheel = (-1.0)*kappa * wheel_base * wheelratio * 180.0/math.pi
+        wheel = (1.0)*kappa * wheel_base * wheelratio * 180.0/math.pi
+        if wheel>self.mmaxwheel:
+            wheel = self.mmaxwheel
+        if wheel<(self.mmaxwheel * (-1.0)):
+            wheel = self.mmaxwheel * (-1.0)
 
-        return acc,wheel
+        return acc,wheel,self.mspeed
         
 
     def FindNearIndex(self,tracemap : mapdata_pb2.tracemap,xgpsimu : gpsimu_pb2.gpsimu) :

src/python/pythondecisiondemo/proto/chassis.proto

@@ -0,0 +1,33 @@
+syntax = "proto2";
+
+package iv;
+
+message chassis
+{
+  optional int64 time = 1; //ms
+  optional int32 EPSMode = 2  [default = 0]; //0 idle 1 Manual 2 Auto
+  optional int32 EPBFault = 3 [default = 0];  //0 No 1 Have Fault
+  optional int32 DriveMode = 4;      //0 Manual 1 Auto
+  optional int32 Shift = 5;  //0 N  1 D   2 R  3 P  hapo:1p2r3n4d
+  optional int32 AEBAvailable = 6;
+  optional int32 CDDAvailable = 7;
+  optional int32 angle_feedback = 8;
+  optional float torque = 9;
+  optional float vel = 10;   //km/h
+  optional float accstep = 11;
+  optional float soc = 12;
+  optional float brake_feedback = 13;
+  optional int32 EPB_feedback = 14;
+  optional int32 EmergencyStop_feedback = 15;
+  optional int32 brakelight_feedback = 16;
+  optional float range_feedback = 17;
+  optional int32 drivectrltype_feedback = 18;
+  optional int32 brakectrltype_feedback = 19;
+  optional int32 epsctrltype_feedback = 20;
+  optional float frontleftwheel_feedback = 21;
+  optional float frontrightwheel_feedback = 22;
+  optional float rearleftwheel_feedback = 23;
+  optional float rearrightwheel_feedback = 24;
+  optional float engine_speed = 25;
+  
+};

src/python/pythondecisiondemo/proto/chassis_pb2.py

@@ -0,0 +1,25 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: chassis.proto
+"""Generated protocol buffer code."""
+from google.protobuf.internal import builder as _builder
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\rchassis.proto\x12\x02iv\"\xdc\x04\n\x07\x63hassis\x12\x0c\n\x04time\x18\x01 \x01(\x03\x12\x12\n\x07\x45PSMode\x18\x02 \x01(\x05:\x01\x30\x12\x13\n\x08\x45PBFault\x18\x03 \x01(\x05:\x01\x30\x12\x11\n\tDriveMode\x18\x04 \x01(\x05\x12\r\n\x05Shift\x18\x05 \x01(\x05\x12\x14\n\x0c\x41\x45\x42\x41vailable\x18\x06 \x01(\x05\x12\x14\n\x0c\x43\x44\x44\x41vailable\x18\x07 \x01(\x05\x12\x16\n\x0e\x61ngle_feedback\x18\x08 \x01(\x05\x12\x0e\n\x06torque\x18\t \x01(\x02\x12\x0b\n\x03vel\x18\n \x01(\x02\x12\x0f\n\x07\x61\x63\x63step\x18\x0b \x01(\x02\x12\x0b\n\x03soc\x18\x0c \x01(\x02\x12\x16\n\x0e\x62rake_feedback\x18\r \x01(\x02\x12\x14\n\x0c\x45PB_feedback\x18\x0e \x01(\x05\x12\x1e\n\x16\x45mergencyStop_feedback\x18\x0f \x01(\x05\x12\x1b\n\x13\x62rakelight_feedback\x18\x10 \x01(\x05\x12\x16\n\x0erange_feedback\x18\x11 \x01(\x02\x12\x1e\n\x16\x64rivectrltype_feedback\x18\x12 \x01(\x05\x12\x1e\n\x16\x62rakectrltype_feedback\x18\x13 \x01(\x05\x12\x1c\n\x14\x65psctrltype_feedback\x18\x14 \x01(\x05\x12\x1f\n\x17\x66rontleftwheel_feedback\x18\x15 \x01(\x02\x12 \n\x18\x66rontrightwheel_feedback\x18\x16 \x01(\x02\x12\x1e\n\x16rearleftwheel_feedback\x18\x17 \x01(\x02\x12\x1f\n\x17rearrightwheel_feedback\x18\x18 \x01(\x02\x12\x14\n\x0c\x65ngine_speed\x18\x19 \x01(\x02')
+
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'chassis_pb2', globals())
+if _descriptor._USE_C_DESCRIPTORS == False:
+
+  DESCRIPTOR._options = None
+  _CHASSIS._serialized_start=22
+  _CHASSIS._serialized_end=626
+# @@protoc_insertion_point(module_scope)

src/python/pythondecisiondemo/proto/decition.proto

@@ -0,0 +1,60 @@
+syntax = "proto2";
+
+package iv.brain;
+
+message decition
+{
+    optional float speed = 1;                //车速
+    optional float wheelAngle = 2;           //横向转向角度
+    optional float wheelSpeed = 3;           //方向盘角速度
+    optional float lateralTorque = 4;        //横向扭矩
+    optional float brake = 5;                //刹车
+    optional float accelerator = 6;          //纵向加速度
+    optional float torque = 7;               //油门
+    optional bool  leftLamp = 8;             //左转向灯
+    optional bool  rightLamp = 9;            //右转向灯
+    optional bool  doubleSpark = 10;          //双闪
+    optional bool  headLight = 11;            //前灯
+    optional bool  highBeam = 12;             //远光灯
+    optional int32  dippedLight = 13;          //近光灯
+    optional bool  tailLight = 14;            //尾灯
+    optional bool  domeLight = 15;            //顶灯
+    optional bool  fogLamp = 16;              //雾灯
+    optional bool  backupLight = 17;          //倒车灯
+    optional bool  brakeLamp = 18;            //制动灯
+    optional int32   engine = 19;               //发动机点火
+    optional int32   mode = 20;                 //模式：自动or手动
+    optional int32  handBrake = 21;            //手刹
+    optional bool  speak = 22;                //喇叭
+    optional int32  door = 23;                 //车门
+    optional int32   gear = 24;                 //挡位
+    optional int32   wiper = 25;                //雨刷
+    optional int32   grade = 26;                 //GE3使用的一个指标
+    optional float   ttc = 27;                  //预测碰撞时间 
+    optional bool   air_enable = 28;                  //空调使能 
+    optional float   air_temp = 29;                  //空调温度 
+    optional float   air_mode = 30;                  //空调模式 
+    optional float   wind_level = 31;                  //空调风力 
+    optional int32   roof_light = 32;                  //顶灯 
+    optional int32   home_light = 33;                  //日光灯 
+    optional float   air_worktime = 34;                  //空调工作时间
+    optional float   air_offtime = 35;                  //空调关闭时间 
+    optional bool   air_on = 36;                  //空调使能 
+	// vv7-tdu
+	optional bool air_ac = 37;
+	optional bool air_circle = 38;
+	optional bool air_auto = 39;
+	optional bool air_off = 40;
+	optional uint32 window_fl = 41;
+	optional uint32 window_fr = 42;
+	optional uint32 window_rl = 43;
+	optional uint32 window_rr = 44;
+    //chang_an_shen_lan
+	optional uint32 angle_mode = 45;  //横向控制激活模式
+	optional uint32 angle_active = 46; //横向控制激活，和上一条同时满足才执行横向请求角度 
+	optional uint32 acc_active = 47; //ACC扭矩请求激活（为1激活，为0不激活），不激活后不执行扭矩请求
+	optional uint32 brake_active = 48; //ACC减速激活（为1制动激活，为0制动不激活）
+	optional uint32 brake_type = 49;//ADC请求类型（制动时为1，其余情况为0）
+	optional float niuju_y = 50; //纵向扭矩
+	optional uint32 auto_mode = 51; //3为自动控制模式
+};

src/python/pythondecisiondemo/proto/gpsimu.proto

@@ -0,0 +1,82 @@
+syntax = "proto2";
+
+package iv.gps;
+
+message pos_accuracy_def
+{
+ optional double latstd = 1;
+ optional double lonstd = 2;
+ optional double hstd = 3;
+};
+
+message vel_accuracy_def
+{
+ optional double vnstd = 1;
+ optional double vestd = 2;
+ optional double vdstd = 3;
+};
+
+message pose_accuracy_def
+{
+ optional double rollstd = 1;
+ optional double pitchstd = 2;
+ optional double yawstd = 3;
+};
+
+message dev_temp_def
+{
+ optional double temp = 1;
+};
+
+message gps_state_def
+{
+ optional int32 pos_state = 1;
+ optional int32 satnum = 2;
+ optional int32 heading_state = 3;
+};
+
+message wheel_state_def
+{
+ optional int32 wheeldata = 1;
+};
+
+
+
+message gpsimu
+{
+ optional double pitch = 1;
+ optional double roll = 2;
+ optional double heading = 3;
+ optional double gyro_x = 4; //惯导x轴角速度
+ optional double gyro_y = 5; //惯导y轴角速度
+ optional double gyro_z = 6; //惯导z轴角速度
+ optional double acce_x = 7;
+ optional double acce_y = 8;
+ optional double acce_z = 9;
+ optional double lat = 10;
+ optional double lon = 11;
+ optional double height = 12;
+ optional double vn = 13;//北向速度
+ optional double ve = 14;//东向速度
+ optional double vd = 15;//地向速度
+ optional int32 state = 16;
+ optional int32 type = 17;
+ optional pos_accuracy_def pos_accuracy = 18;
+ optional vel_accuracy_def vel_accuracy = 19;
+ optional dev_temp_def dev_temp = 20;
+ optional gps_state_def gps_state = 21;
+ optional wheel_state_def wheel_state = 22;
+ optional double time = 23;
+ optional bytes check = 24;
+ optional pose_accuracy_def pose_accuracy = 25;
+ optional int64 msgtime = 26;
+ optional double rtk_state = 27;
+ optional double ins_state = 28;
+ optional double acc_calc = 29;
+ optional int32 satnum1 = 30;
+ optional int32 satnum2 = 31;
+ optional int32 gpsweek = 32; //from 1980-1-6 weeks.
+ optional int32 gpstime = 33; //from sunday 0:00:00 seconds.
+ optional double speed = 34;
+
+};

src/python/pythondecisiondemo/proto/mapdata.proto

@@ -0,0 +1,64 @@
+syntax = "proto2";
+
+package iv.map;
+
+message mappoint
+{
+        optional double gps_lat = 1 [default=0.0];//纬度
+        optional double gps_lng  = 2 [default=0.0];//经度
+
+        optional double gps_x  = 3 [default=0];   
+        optional double gps_y  = 4 [default=0];
+        optional double gps_z  = 5 [default=0];
+
+        optional double ins_heading_angle  = 6 [default=0];	//航向角
+
+
+        optional int32 speed_mode  = 7 [default=0];
+        optional int32 mode2  = 8 [default=0];
+        optional double speed  = 9 [default=0];			//速度  若导航点则为导航预设速度  若为当前点则为当前车速
+
+        optional int32 roadMode = 10[default=0];
+        optional int32 runMode = 11[default=0];
+        optional int32 roadSum = 12[default=1];
+        optional int32 roadOri = 13[default=0];
+
+        optional double mfLaneWidth  = 14 [default=3.5]; // Current Lane Width
+
+        optional double mfDisToLaneLeft  = 15 [default=1.8]; //Distance to Lane Left
+        optional int32 mnLaneChangeMark  = 16 [default=0]; //1 to Left 0 not change   -1 to right
+        optional double mfDisToRoadLeft  = 17 [default=1.8]; //Distance to Road Left
+        optional double mfRoadWidth  = 18 [default=3.5]; // Road Width
+
+        optional bool mbInLaneAvoid  = 19 [default=true]; //if true suport In Lane Avoid
+        optional  double gps_lat_avoidleft = 20;
+        optional double gps_lng_avoidleft = 21;
+        optional double gps_lat_avoidright = 22;
+        optional double gps_lng_avoidright = 23;
+        optional double gps_x_avoidleft = 24;
+        optional double gps_y_avoidleft = 25;
+        optional double gps_x_avoidright = 26;
+        optional double gps_y_avoidright = 27;
+
+        optional bool mbnoavoid  = 28 [default=false]; //If true this point is no avoid.
+        optional double mfCurvature  = 29 [default=0];
+	
+	optional double rel_x = 30;  //relative x
+	optional double rel_y = 31;   //relative y
+	optional double rel_z = 32;   //relative z
+	optional double rel_yaw = 33;  //yaw
+	optional int32 laneid = 34;  // -1 -2 -3 right lane   1 2 3 left lane
+	optional int32 leftlanecout = 35; 
+	optional int32 rightlanecount = 36;
+	optional int32 roadid = 37;
+	optional int32 index = 38;
+
+
+};
+
+
+message tracemap
+{
+	repeated mappoint point = 1;
+	optional int32 nState  = 2;  
+};

src/python/pythondecisiondemo/proto/object.proto

@@ -0,0 +1,51 @@
+syntax = "proto2";
+
+package iv.lidar;
+
+message PointXYZI {
+  optional float x = 1 [default = nan];
+  optional float y = 2 [default = nan];
+  optional float z = 3 [default = nan];
+  optional float i = 4 [default = 0];
+};
+
+message PointXYZ {
+  optional float x = 1 [default = nan];
+  optional float y = 2 [default = nan];
+  optional float z = 3 [default = nan];
+};
+
+message Dimension {
+  optional float x = 1 [default = 0];
+  optional float y = 2 [default = 0];
+  optional float z = 3 [default = 0];
+};
+
+message VelXY {
+  optional float x =1 [default = 0];
+  optional float y =2 [default = 0];
+};
+
+message lidarobject
+{
+  optional PointXYZ min_point = 1;
+  optional PointXYZ max_point = 2;
+  optional PointXYZ centroid = 3;
+  optional Dimension dimensions = 4;
+  optional PointXYZ position = 5;
+  optional double angle =6;
+  optional double velocity_linear_x = 7;
+  optional double acceleration_linear_y = 8;
+  optional double timestamp = 9;
+  optional double tyaw = 10;
+  required int32 mnType = 11;
+  optional int32 id = 12;
+  optional int32 behavior_state = 13;
+  optional float score = 14;
+  optional bool velocity_reliable = 15;
+  optional bool pose_reliable = 16;
+  repeated float type_probs = 17;
+  repeated PointXYZI cloud = 18;
+  optional string type_name = 19;
+  optional VelXY vel_relative = 20; //相对速度
+};

src/python/pythondecisiondemo/proto/object_pb2.py

@@ -0,0 +1,33 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: object.proto
+"""Generated protocol buffer code."""
+from google.protobuf.internal import builder as _builder
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x0cobject.proto\x12\x08iv.lidar\"I\n\tPointXYZI\x12\x0e\n\x01x\x18\x01 \x01(\x02:\x03nan\x12\x0e\n\x01y\x18\x02 \x01(\x02:\x03nan\x12\x0e\n\x01z\x18\x03 \x01(\x02:\x03nan\x12\x0c\n\x01i\x18\x04 \x01(\x02:\x01\x30\":\n\x08PointXYZ\x12\x0e\n\x01x\x18\x01 \x01(\x02:\x03nan\x12\x0e\n\x01y\x18\x02 \x01(\x02:\x03nan\x12\x0e\n\x01z\x18\x03 \x01(\x02:\x03nan\"5\n\tDimension\x12\x0c\n\x01x\x18\x01 \x01(\x02:\x01\x30\x12\x0c\n\x01y\x18\x02 \x01(\x02:\x01\x30\x12\x0c\n\x01z\x18\x03 \x01(\x02:\x01\x30\"#\n\x05VelXY\x12\x0c\n\x01x\x18\x01 \x01(\x02:\x01\x30\x12\x0c\n\x01y\x18\x02 \x01(\x02:\x01\x30\"\xa1\x04\n\x0blidarobject\x12%\n\tmin_point\x18\x01 \x01(\x0b\x32\x12.iv.lidar.PointXYZ\x12%\n\tmax_point\x18\x02 \x01(\x0b\x32\x12.iv.lidar.PointXYZ\x12$\n\x08\x63\x65ntroid\x18\x03 \x01(\x0b\x32\x12.iv.lidar.PointXYZ\x12\'\n\ndimensions\x18\x04 \x01(\x0b\x32\x13.iv.lidar.Dimension\x12$\n\x08position\x18\x05 \x01(\x0b\x32\x12.iv.lidar.PointXYZ\x12\r\n\x05\x61ngle\x18\x06 \x01(\x01\x12\x19\n\x11velocity_linear_x\x18\x07 \x01(\x01\x12\x1d\n\x15\x61\x63\x63\x65leration_linear_y\x18\x08 \x01(\x01\x12\x11\n\ttimestamp\x18\t \x01(\x01\x12\x0c\n\x04tyaw\x18\n \x01(\x01\x12\x0e\n\x06mnType\x18\x0b \x02(\x05\x12\n\n\x02id\x18\x0c \x01(\x05\x12\x16\n\x0e\x62\x65havior_state\x18\r \x01(\x05\x12\r\n\x05score\x18\x0e \x01(\x02\x12\x19\n\x11velocity_reliable\x18\x0f \x01(\x08\x12\x15\n\rpose_reliable\x18\x10 \x01(\x08\x12\x12\n\ntype_probs\x18\x11 \x03(\x02\x12\"\n\x05\x63loud\x18\x12 \x03(\x0b\x32\x13.iv.lidar.PointXYZI\x12\x11\n\ttype_name\x18\x13 \x01(\t\x12%\n\x0cvel_relative\x18\x14 \x01(\x0b\x32\x0f.iv.lidar.VelXY')
+
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'object_pb2', globals())
+if _descriptor._USE_C_DESCRIPTORS == False:
+
+  DESCRIPTOR._options = None
+  _POINTXYZI._serialized_start=26
+  _POINTXYZI._serialized_end=99
+  _POINTXYZ._serialized_start=101
+  _POINTXYZ._serialized_end=159
+  _DIMENSION._serialized_start=161
+  _DIMENSION._serialized_end=214
+  _VELXY._serialized_start=216
+  _VELXY._serialized_end=251
+  _LIDAROBJECT._serialized_start=254
+  _LIDAROBJECT._serialized_end=799
+# @@protoc_insertion_point(module_scope)

src/python/pythondecisiondemo/proto/objectarray.proto

@@ -0,0 +1,11 @@
+syntax = "proto2";
+
+package iv.lidar;
+
+import "object.proto";
+
+message objectarray
+{
+  repeated lidarobject obj = 1;
+  optional double timestamp = 2;
+};

src/python/pythondecisiondemo/proto/objectarray_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: objectarray.proto
+"""Generated protocol buffer code."""
+from google.protobuf.internal import builder as _builder
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+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')
+
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'objectarray_pb2', globals())
+if _descriptor._USE_C_DESCRIPTORS == False:
+
+  DESCRIPTOR._options = None
+  _OBJECTARRAY._serialized_start=45
+  _OBJECTARRAY._serialized_end=113
+# @@protoc_insertion_point(module_scope)

src/python/pythondecisiondemo/proto/radarobject.proto

@@ -0,0 +1,61 @@
+syntax = "proto2";
+
+package iv.radar;
+
+message radarobject
+{
+  required double x = 1; //直角坐标位置，右为正，米
+  required double y = 2; //直角坐标位置，前为正，米
+  required double vx = 3; //直角坐标速度，右为正，米每秒
+  required double vy = 4; //直角坐标速度，前为正，米每秒
+  required double vel = 5; //速度标量，始终为正，米每秒
+  required bool bValid = 6; //数据可信标志
+
+  // Delphi ESR
+  optional double Range =7; //极坐标下距离
+  optional double Range_Rate =8; //纵向速度，极坐标，远离为正，米每秒
+  optional double Range_Accel =9; //纵向加速度，极坐标，远离为正，米每二次方秒
+  optional double Angle =10; //位置角度，极坐标，顺时针正，正前为0，度
+  optional double Width =11; //宽度，米，分辨率0.5米
+  optional bool Grouping_Changed =12; //组别变化
+  optional bool Oncoming =13; //相向移动
+  optional double Lat_Rate =14; //横向速度，极坐标，逆时针正，米每秒
+  optional int32 Med_Range_Mode =15; //目标被追踪模式，0无追踪，1中距雷达追踪，2长距雷达追踪，3中、长距追踪
+  optional int32 Track_Status =16; //追踪状态，0无目标，1新的目标，2新的更新过的目标，3更新过的目标，4平稳运动的目标，5渐融的目标，6不可信的平稳运动目标，7新的平稳运动目标
+  optional bool Bridge_Object =17; //桥梁标志
+  optional bool Moving =18; //运动
+  optional bool Fast_Movable =19; //快速运动
+  optional bool Slow_Movable =20; //慢速运动
+  optional int32 Power =21; //信号反射强度，单位dB
+  optional int32 Detect_Valid_Level = 22;//srr目标置信度
+  optional bool Detect_Status = 23;//srr目标检测状态，为1目标有效，置信度有意义
+
+  // Continental ARS408/SRR308
+  optional int32 ID =24;
+  optional int32 Dynamic_Property =25; //动态属性 0运动 1静止 2来向 3静止候选 4未知 5横穿静止 6横穿运动 7停止（运动过）
+  optional double RCS_RadarCrossSection =26; //目标对电波的散射面积
+  optional double DistLong_RMS =27; //纵向距离标准差 即真值=纵向距离+/-标准差 SRR308不可用
+  optional double DistLat_RMS =28; //横向距离标准差 SRR308不可用
+  optional double VrelLong_RMS =29; //纵向速度标准差 SRR308不可用
+  optional double VrelLat_RMS =30; //横向速度标准差 SRR308不可用
+
+  optional double Cluster_Pdh0 =31; //Cluster目标虚警概率
+  optional int32 Cluster_AmbigState =32; //多普勒径向速度解模糊状态 >=3时 cluster相对可信 SRR308不可用
+  optional int32 Cluster_InvalidState =33; //Cluster目标有效性状态 1、2、3、5、6、7、D、E无效，其他的是有效目标但可能有属性缺陷 308不可用
+  optional int32 Cluster_PropertiesBitfield =34; //SRR308专属 目标状态属性字段 需要按位解析 0x0为无属性 bit0静态 bit1动态 bit2模糊的 bit4近场波束扫描到的 bit6护栏后的目标
+  optional int32 Cluster_InvalidReasonBitfield =35; //SRR308专属 目标无效原因字段 需要按位解析 0x0表示有效目标 bit1低RCS动态 bit2低RCS静态 bit3速度不可信 bit4距离不可信 bit5目标是多径虚影
+
+  optional double Object_ArelLatRMS =36; //横向相对加速度标准差
+  optional double Object_ArelLongRMS =37; //纵向相对加速度标准差
+  optional double Object_OrientationRMS =38; //航向角标准差
+  optional int32 Object_MeasureState =39; //测量状态 0被删除的目标 1新创建的 2测量到的 3预测出的 4并如其他聚类的 5从聚类中分离出的
+  optional double Object_ProbOfExist =40; //目标存在概率
+  optional double Object_ArelLong =41; //纵向加速度
+  optional double Object_ArelLat =42; //横向加速度
+  optional double Object_OrientationAngle =43; //航向角
+  optional int32 Object_Class =44; //目标类别 0point 1car 2truck 3pedestrian 4motorcycle 5bicycle 6wide/wall 7reserved
+  optional double Object_Length =45; //目标长度
+  optional double Object_Width =46; //目标宽度
+};
+
+

src/python/pythondecisiondemo/proto/radarobjectarray.proto

@@ -0,0 +1,51 @@
+
+syntax = "proto2";
+
+package iv.radar;
+
+import "radarobject.proto";
+
+message radarobjectarray
+{
+  repeated radarobject obj = 1;
+  optional int64 mstime = 2;
+
+  //Delphi ESR
+  optional int32 ACC_Sta_Obj_ID =3; //路线上自适应巡航静态或相对靠近目标推荐id
+  optional int32 ACC_Mov_Obj_ID =4; //路线上自适应巡航动态或可动目标推荐id
+  optional int32 CMbB_Sta_Obj_ID =5; //路线上刹车减缓碰撞静态目标推荐id
+  optional int32 CMbB_Mov_Obj_ID =6; //路线上刹车减缓碰撞动态目标推荐id
+  optional int32 FCW_Sta_Obj_ID =7; //路线上前碰撞预警静态目标推荐id
+  optional int32 FCW_Mov_Obj_ID =8; //路线上前碰撞预警动态目标推荐id
+  optional bool Grating_Lobe_Det =9; //acc目标为栅瓣目标，可能并非路线上最近物体，需谨慎判断
+  optional bool Truck_Target_Det =10; //acc目标是卡车类，可能并非路线上的最近物体，需谨慎判断
+
+  optional int32 Water_Spray_Target_ID = 11; //最近的溅水目标，该目标可能是虚影（雨雪），也可能是实物（车轮溅水）
+  optional double Filtered_XOHP_ACC_CIPV = 12; //路线上最近ACC目标的滤波后的在主车驶路线上的横向偏移，单位米
+  optional int32 Path_ID_ACC_2 = 13; //路线上第二近ACC目标id
+  optional int32 Path_ID_ACC_3 = 14; //路线上第三近ACC目标id
+
+  optional bool Internal_Error = 15; //雷达内部错误标志，0为正常
+  optional bool Range_Perf_Error = 16; //雷达受遮挡错误标志，0为正常
+  optional bool Overheat_Error = 17; //雷达过热标志，0为正常
+
+  optional double Valid_LR_Range =18; //长距雷达可信距离
+  optional double Valid_LR_Range_Rate =19; //长距雷达可信速度
+  optional double Valid_LR_Angle = 20; //长距雷达可信角度
+  optional double Valid_LR_Power =21; //长距雷达可信目标反射强度
+
+  optional double Valid_MR_Range =22; //中距雷达可信距离
+  optional double Valid_MR_Range_Rate =23; //中距雷达可信速度
+  optional double Valid_MR_Angle = 24; //中距雷达可信角度
+  optional double Valid_MR_Power =25; //中距雷达可信目标反射强度
+  
+  //Continental ARS408/SRR308
+  optional bool Persistent_Error =26; //持续性错误标志，重启无法恢复
+  optional bool Interference_Error =27; //检测到干扰
+  optional bool Temperature_Error =28; //温度过高过低报警
+  optional bool Temporary_Error =29; //临时错误，重启可消除
+  optional bool Voltage_Error =30; //电压异常
+  optional bool Speed_Loss =31; //车速信息丢失
+  optional bool Yaw_Rate_Loss =32; //航向角速度信息丢失
+  optional string Radar_Version =33; //版本号  
+};

src/python/pythondecisiondemo/proto/radarobjectarray_pb2.py

@@ -0,0 +1,26 @@
+# -*- coding: utf-8 -*-
+# Generated by the protocol buffer compiler.  DO NOT EDIT!
+# source: radarobjectarray.proto
+"""Generated protocol buffer code."""
+from google.protobuf.internal import builder as _builder
+from google.protobuf import descriptor as _descriptor
+from google.protobuf import descriptor_pool as _descriptor_pool
+from google.protobuf import symbol_database as _symbol_database
+# @@protoc_insertion_point(imports)
+
+_sym_db = _symbol_database.Default()
+
+
+import radarobject_pb2 as radarobject__pb2
+
+
+DESCRIPTOR = _descriptor_pool.Default().AddSerializedFile(b'\n\x16radarobjectarray.proto\x12\x08iv.radar\x1a\x11radarobject.proto\"\xd0\x06\n\x10radarobjectarray\x12\"\n\x03obj\x18\x01 \x03(\x0b\x32\x15.iv.radar.radarobject\x12\x0e\n\x06mstime\x18\x02 \x01(\x03\x12\x16\n\x0e\x41\x43\x43_Sta_Obj_ID\x18\x03 \x01(\x05\x12\x16\n\x0e\x41\x43\x43_Mov_Obj_ID\x18\x04 \x01(\x05\x12\x17\n\x0f\x43MbB_Sta_Obj_ID\x18\x05 \x01(\x05\x12\x17\n\x0f\x43MbB_Mov_Obj_ID\x18\x06 \x01(\x05\x12\x16\n\x0e\x46\x43W_Sta_Obj_ID\x18\x07 \x01(\x05\x12\x16\n\x0e\x46\x43W_Mov_Obj_ID\x18\x08 \x01(\x05\x12\x18\n\x10Grating_Lobe_Det\x18\t \x01(\x08\x12\x18\n\x10Truck_Target_Det\x18\n \x01(\x08\x12\x1d\n\x15Water_Spray_Target_ID\x18\x0b \x01(\x05\x12\x1e\n\x16\x46iltered_XOHP_ACC_CIPV\x18\x0c \x01(\x01\x12\x15\n\rPath_ID_ACC_2\x18\r \x01(\x05\x12\x15\n\rPath_ID_ACC_3\x18\x0e \x01(\x05\x12\x16\n\x0eInternal_Error\x18\x0f \x01(\x08\x12\x18\n\x10Range_Perf_Error\x18\x10 \x01(\x08\x12\x16\n\x0eOverheat_Error\x18\x11 \x01(\x08\x12\x16\n\x0eValid_LR_Range\x18\x12 \x01(\x01\x12\x1b\n\x13Valid_LR_Range_Rate\x18\x13 \x01(\x01\x12\x16\n\x0eValid_LR_Angle\x18\x14 \x01(\x01\x12\x16\n\x0eValid_LR_Power\x18\x15 \x01(\x01\x12\x16\n\x0eValid_MR_Range\x18\x16 \x01(\x01\x12\x1b\n\x13Valid_MR_Range_Rate\x18\x17 \x01(\x01\x12\x16\n\x0eValid_MR_Angle\x18\x18 \x01(\x01\x12\x16\n\x0eValid_MR_Power\x18\x19 \x01(\x01\x12\x18\n\x10Persistent_Error\x18\x1a \x01(\x08\x12\x1a\n\x12Interference_Error\x18\x1b \x01(\x08\x12\x19\n\x11Temperature_Error\x18\x1c \x01(\x08\x12\x17\n\x0fTemporary_Error\x18\x1d \x01(\x08\x12\x15\n\rVoltage_Error\x18\x1e \x01(\x08\x12\x12\n\nSpeed_Loss\x18\x1f \x01(\x08\x12\x15\n\rYaw_Rate_Loss\x18  \x01(\x08\x12\x15\n\rRadar_Version\x18! \x01(\t')
+
+_builder.BuildMessageAndEnumDescriptors(DESCRIPTOR, globals())
+_builder.BuildTopDescriptorsAndMessages(DESCRIPTOR, 'radarobjectarray_pb2', globals())
+if _descriptor._USE_C_DESCRIPTORS == False:
+
+  DESCRIPTOR._options = None
+  _RADAROBJECTARRAY._serialized_start=56
+  _RADAROBJECTARRAY._serialized_end=904
+# @@protoc_insertion_point(module_scope)

src/python/pythondecisiondemo/pythondecisiondemo.py

@@ -2,8 +2,10 @@ import modulecommpython
 import numpy as np  
 import time
 
-import gpsimu_pb2
-import mapdata_pb2
+import proto.gpsimu_pb2 as gpsimu_pb2
+#import gpsimu_pb2
+import proto.mapdata_pb2 as mapdata_pb2
+import proto.decition_pb2 as decition_pb2
 
 from PyModuleCommModule import PyModuleComm
 from decisiondemo import DecisionDemo
@@ -51,7 +53,11 @@ def MakeDecision(md : PyModuleComm, mapdata : mapdata_pb2.tracemap, gpsimu : gps
         print(" no tracemap")
         return
     print("map1 point lenth: ",length)
-    gdecision.CalcDecision(mapdata,gpsimu)
+    xdecision = gdecision.CalcDecision(mapdata,gpsimu)
+    serialized_str = xdecision.SerializeToString()  
+    serialized_array = np.frombuffer(serialized_str, dtype=np.uint8)  
+    length = len(serialized_array)
+    md.SendData(serialized_array,length)
     pass
 
     
@@ -61,7 +67,6 @@ def main():
     # 初始化一个变量  
     count = 0
 
-
     count = 3.5/1.5
     count = int(count)
     print("count = ",count)  
@@ -72,12 +77,12 @@ def main():
     mmap = PyModuleComm("newtracemap")
     mmap.RegisterRecv(mapdata_callback)
 
-    md = PyModuleComm("decision")
+    md = PyModuleComm("deciton")
     md.RegiseterSend(100000,1)
     # 使用while循环，只要count小于10，就继续循环  
     while count < 10:
         MakeDecision(md,msg_mapdata,msg_gpsimu)
-        time.sleep(1.0)
+        time.sleep(0.05)
 
   
     print("循环结束！")