commit

aarch64/pjisuv/master/service/produce_window.go

@@ -1260,17 +1260,7 @@ func ProduceWindow() {
 					Node:  commonConfig.RosNode,
 					Topic: topic,
 					Callback: func(data *pjisuv_msgs.CommonVehicleCmd) {
-						// 更新共享变量
-						mutexOfPjControlPub.RLock()
-						{
-							pjisuvParam.NumCountPjiControlCommandOfPjControlPub++
-							if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 {
-								pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
-								pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
-								pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0
-							}
-						}
-						mutexOfPjControlPub.RUnlock()
+
 						subscribersTimeMutexes[i].Lock()
 						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
 							subscribersMutexes[i].Lock()
@@ -1314,6 +1304,17 @@ func ProduceWindow() {
 							subscribersMutexes[i].Unlock()
 						}
 						subscribersTimeMutexes[i].Unlock()
+						// 更新共享变量
+						mutexOfPjControlPub.RLock()
+						{
+							pjisuvParam.NumCountPjiControlCommandOfPjControlPub++
+							if pjisuvParam.NumCountPjiControlCommandOfPjControlPub == 10 {
+								pjisuvParam.EgoSteeringCmdOfPjControlPub = append(pjisuvParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
+								pjisuvParam.EgoThrottleCmdOfPjControlPub = append(pjisuvParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
+								pjisuvParam.NumCountPjiControlCommandOfPjControlPub = 0
+							}
+						}
+						mutexOfPjControlPub.RUnlock()
 					},
 				})
 			}
@@ -1716,24 +1717,7 @@ func ProduceWindow() {
 					Node:  commonConfig.RosNode,
 					Topic: topic,
 					Callback: func(data *pjisuv_msgs.PerceptionObjects) {
-						// 更新共享变量
-						mutexOfTpperception.RLock()
-						{
-							for _, obj := range data.Objs {
-								if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
-									continue
-								}
-								// 检查 ObjDicOfTpperception 是否为 nil，如果是，则初始化它
-								if pjisuvParam.ObjDicOfTpperception == nil {
-									pjisuvParam.ObjDicOfTpperception = make(map[uint32][]float32)
-								}
-								if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
-									pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
-								}
-								pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
-							}
-						}
-						mutexOfTpperception.RUnlock()
+
 						subscribersTimeMutexes[i].Lock()
 						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
 							subscribersMutexes[i].Lock()
@@ -1778,9 +1762,21 @@ func ProduceWindow() {
 						}
 						subscribersTimeMutexes[i].Unlock()
 						// -------- 触发后更新共享变量
+						// 更新共享变量
 						mutexOfTpperception.RLock()
 						{
 							for _, obj := range data.Objs {
+								if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
+									continue
+								}
+								// 检查 ObjDicOfTpperception 是否为 nil，如果是，则初始化它
+								if pjisuvParam.ObjDicOfTpperception == nil {
+									pjisuvParam.ObjDicOfTpperception = make(map[uint32][]float32)
+								}
+								if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
+									pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
+								}
+								pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
 								pjisuvParam.ObjTypeDicOfTpperception[obj.Id] = obj.Type
 								pjisuvParam.ObjSpeedDicOfTpperception[obj.Id] = math.Pow(math.Pow(float64(obj.Vxabs), 2)+math.Pow(float64(obj.Vyabs), 2), 0.5)
 							}
@@ -2253,11 +2249,6 @@ func ProduceWindow() {
 					Node:  commonConfig.RosNode,
 					Topic: topic,
 					Callback: func(data *geometry_msgs.Point) {
-						// 更新共享变量
-
-						shareVars.Store("EndPointX", data.X)
-						shareVars.Store("EndPointY", data.Y)
-
 						subscribersTimeMutexes[i].Lock()
 						if time.Since(subscribersTimes[i]).Seconds() > triggerInterval {
 							subscribersMutexes[i].Lock()
@@ -2301,6 +2292,10 @@ func ProduceWindow() {
 							subscribersMutexes[i].Unlock()
 						}
 						subscribersTimeMutexes[i].Unlock()
+
+						// 更新共享变量
+						shareVars.Store("EndPointX", data.X)
+						shareVars.Store("EndPointY", data.Y)
 					},
 				})
 			}