change decition_speedlimit,solve speedlimit problem. change cameradecison, add speed feedback.

src/decition/decition_speedlimit/decisionspeedlimit.cpp

@@ -5,8 +5,24 @@
 decisionspeedlimit::decisionspeedlimit(double fspeedlimit) {
 
     mfspeedlimit = fspeedlimit;
-    ModuleFun xFunchassis = std::bind(&decisionspeedlimit::ListenChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
-    mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",xFunchassis);
+ //   ModuleFun xFunchassis = std::bind(&decisionspeedlimit::ListenChassis,this,std::placeholders::_1,std::placeholders::_2,std::placeholders::_3,std::placeholders::_4,std::placeholders::_5);
+    mpachassis = iv::modulecomm::RegisterRecvPlus("chassis",[this](const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
+
+        iv::chassis xchassis;
+        if(!xchassis.ParseFromArray(strdata,nSize))
+        {
+            std::cout<<"Chassis Parse  Fail."<<std::endl;
+            return;
+        }
+
+        if(xchassis.has_vel())
+        {
+            mfNowSpeed = xchassis.vel();
+            mnLastChassisUpdateTime = std::chrono::system_clock::now().time_since_epoch().count();
+        }
+
+        (void)index;(void)dt;(void)strmemname;
+    });
 
     mpadecision = iv::modulecomm::RegisterSend("deciton",1000,1);
 
@@ -22,24 +38,30 @@ decisionspeedlimit::~decisionspeedlimit(){
 }
 
 
-void decisionspeedlimit::ListenChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
+//void decisionspeedlimit::ListenChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
 
-    iv::chassis xchassis;
-    if(!xchassis.ParseFromArray(strdata,nSize))
-    {
-        std::cout<<"Chassis Parse  Fail."<<std::endl;
-        return;
-    }
+//    iv::chassis xchassis;
+//    if(!xchassis.ParseFromArray(strdata,nSize))
+//    {
+//        std::cout<<"Chassis Parse  Fail."<<std::endl;
+//        return;
+//    }
 
-    mmutexchassis.lock();
-    mnLastChassisUpdateTime = std::chrono::system_clock::now().time_since_epoch().count();
-    mchassis.CopyFrom(xchassis);
-    mmutexchassis.unlock();
+//    mmutexchassis.lock();
+//    mnLastChassisUpdateTime = std::chrono::system_clock::now().time_since_epoch().count();
+//    mchassis.CopyFrom(xchassis);
+//    if(mchassis.has_vel())
+//    {
+//        mfNowSpeed = mchassis.vel();
+//    }
+//    mmutexchassis.unlock();
 
-    (void)index;
-    (void)dt;
-    (void)strmemname;
-}
+
+
+//    (void)index;
+//    (void)dt;
+//    (void)strmemname;
+//}
 
 void decisionspeedlimit::ListenDecsionSpeedLimit(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname){
     (void)index;
@@ -60,31 +82,32 @@ void decisionspeedlimit::ListenDecsionSpeedLimit(const char * strdata,const unsi
 
 
 
-    iv::chassis xchassis;
-    int64_t nlastchassis = 0;
-    mmutexchassis.lock();
-    xchassis.CopyFrom(xchassis);
-    nlastchassis = mnLastChassisUpdateTime;
-    mmutexchassis.unlock();
+//    iv::chassis xchassis;
+//    int64_t nlastchassis = 0;
+//    mmutexchassis.lock();
+//    xchassis.CopyFrom(xchassis);
+//    nlastchassis = mnLastChassisUpdateTime;
+//    mmutexchassis.unlock();
 
     int64_t nnow = std::chrono::system_clock::now().time_since_epoch().count();
     if(ftorque > 0)
     {
-        if(nlastchassis == 0)
+        if(mnLastChassisUpdateTime == 0)
         {
             ftorque = 0;
             std::cout<<nnow<<" no chassis message, limit torque to zero."<<std::endl;
         }
-        if(abs(nnow - nlastchassis) >  1e9)
+        if(abs(nnow - mnLastChassisUpdateTime) >  1e9)
         {
             ftorque = 0;
             std::cout<<nnow<<" no chassis message more than 1 second, limit torque to zero."<<std::endl;
         }
         else
         {
-            if(xchassis.vel() > mfspeedlimit)
+            if(mfNowSpeed > mfspeedlimit)
             {
                 ftorque = 0;
+                std::cout<<std::chrono::system_clock::now().time_since_epoch().count()<<" Now Speed: "<<mfNowSpeed<<std::endl;
                 std::cout<<" speed more than "<<mfspeedlimit<<" ,so limit torque to zero."<<std::endl;
             }
         }

src/decition/decition_speedlimit/decisionspeedlimit.h

@@ -16,7 +16,7 @@ public:
     ~decisionspeedlimit();
 
 private:
-    void ListenChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
+//    void ListenChassis(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
     void ListenDecsionSpeedLimit(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname);
 
 private:
@@ -31,6 +31,8 @@ private:
 
     double mfspeedlimit = 15; //speed  limit
 
+    double mfNowSpeed = 0;
+
 
 
 };

src/python/cameradecision/cameradecision.py

@@ -3,6 +3,7 @@
 
 import proto.cameraobjectarray_pb2 as cameraobjectarray_pb2
 import proto.decitionspeedlimit_pb2 as decitionspeedlimit_pb2
+import proto.gpsimu_pb2 as gpsimu_pb2   # 2024.10.14
 
 import math
 from typing import List  
@@ -36,7 +37,11 @@ class CameraDecision:
         self.mvehwidth = 2.3  #从车宽
         pass
 
-    def CalcDecision(self,xobjarray : cameraobjectarray_pb2.cameraobjectarray):
+    def CalcDecision(self,xobjarray : cameraobjectarray_pb2.cameraobjectarray,xgpsimu: gpsimu_pb2.gpsimu):     # 2024.10.14
+
+        realspeed = 3.6 * math.sqrt(math.pow(xgpsimu.vn,2) + math.pow(xgpsimu.ve,2))  #获取当前车速  # 2024.10.14
+
+
 
         nobjsize = len(xobjarray.obj)
         for pobj in xobjarray.obj:

src/python/cameradecision/cameradecisionmain.py

@@ -6,12 +6,14 @@ import sys
 
 import proto.cameraobjectarray_pb2 as cameraobjectarray_pb2
 import proto.decitionspeedlimit_pb2 as decitionspeedlimit_pb2
+import proto.gpsimu_pb2 as gpsimu_pb2   # 2024.10.14
 
 from PyModuleCommModule import PyModuleComm
 from cameradecision import CameraDecision
 
 
-
+time_gpsimu = 0
+msg_gpsimu = gpsimu_pb2.gpsimu()
 
 time_objectarray = 0
 msg_cameraobjectarray = cameraobjectarray_pb2.cameraobjectarray()
@@ -19,6 +21,19 @@ msg_cameraobjectarray = cameraobjectarray_pb2.cameraobjectarray()
 gdecision = CameraDecision()
 
 
+# 2024.10.14
+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]
+    databytes = sub_arr.tobytes()
+    msg = gpsimu_pb2.gpsimu()
+    msg.ParseFromString(databytes)
+    msg_gpsimu = msg
+    time_gpsimu = time
+# 2024.10.14
 
 def cameraobjectarray_callback(arr : np,nsize,time):
     global msg_cameraobjectarray
@@ -36,13 +51,13 @@ def cameraobjectarray_callback(arr : np,nsize,time):
 def SendDefDecision(md : PyModuleComm):
     pass
 
-def MakeDecision(md : PyModuleComm,  xobjarray: cameraobjectarray_pb2.cameraobjectarray):
+def MakeDecision(md : PyModuleComm,  xobjarray: cameraobjectarray_pb2.cameraobjectarray,xgpsimu: gpsimu_pb2.gpsimu):  # 2024.10.14
     global gdecision
     if time_objectarray == 0 :
         print(" no object")
         return
 #    print("map1 point lenth: ",length)
-    xdecision = gdecision.CalcDecision(xobjarray)
+    xdecision = gdecision.CalcDecision(xobjarray,xgpsimu)  # 2024.10.14
     serialized_str = xdecision.SerializeToString()  
     serialized_array = np.frombuffer(serialized_str, dtype=np.uint8)  
     length = len(serialized_array)
@@ -60,6 +75,8 @@ def main():
     print("count = ",count)  
 
 
+    mc = PyModuleComm("hcp2_gpsimu")  # 2024.10.14
+    mc.RegisterRecv(gpsimu_callback)  # 2024.10.14
 
     mobj = PyModuleComm("signarray")
     mobj.RegisterRecv(cameraobjectarray_callback)
@@ -68,7 +85,7 @@ def main():
     md.RegiseterSend(100000,1)
     # 使用while循环，只要count小于10，就继续循环  
     while count < 10:
-        MakeDecision(md,msg_cameraobjectarray)
+        MakeDecision(md,msg_cameraobjectarray,msg_gpsimu)
         time.sleep(0.05)
 
   

src/python/cameradecision/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/tool/tool_service_maintain/main.cpp

@@ -10,7 +10,7 @@
 
 #endif
 
-std::string gstrserverip = "192.168.1.102";
+std::string gstrserverip = "127.0.0.1";//"192.168.1.102";
 
 void LoadServerIP()
 {