|
|
@@ -21,7 +21,7 @@ import (
|
|
|
|
|
|
// 所有共享变量
|
|
|
var (
|
|
|
- extendParam commonEntity.PjisuvParam
|
|
|
+ pjisuvParam commonEntity.PjisuvParam
|
|
|
// /cicv_location
|
|
|
mutexOfCicvLocation sync.RWMutex
|
|
|
// /tpperception
|
|
|
@@ -30,6 +30,8 @@ var (
|
|
|
mutexOfPjControlPub sync.RWMutex
|
|
|
// /data_read
|
|
|
mutexOfDataRead sync.RWMutex
|
|
|
+ // /pj_vehicle_fdb_pub
|
|
|
+ mutexOfPjVehicleFdbPub sync.RWMutex
|
|
|
)
|
|
|
|
|
|
// PrepareTimeWindowProducerQueue 负责监听所有主题并修改时间窗口
|
|
|
@@ -48,7 +50,7 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
for {
|
|
|
time.Sleep(time.Duration(3500) * time.Millisecond)
|
|
|
for _, f := range masterConfig.RuleOfCicvExtend {
|
|
|
- label := f(extendParam)
|
|
|
+ label := f(pjisuvParam)
|
|
|
if label != "" {
|
|
|
saveTimeWindow(label, util.GetNowTimeCustom(), commonEntity.GetLastTimeWindow())
|
|
|
break
|
|
|
@@ -367,10 +369,13 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
// 更新共享变量
|
|
|
mutexOfCicvLocation.RLock()
|
|
|
{
|
|
|
- extendParam.VelocityYOfCicvLocation = data.VelocityX
|
|
|
- extendParam.VelocityYOfCicvLocation = data.VelocityY
|
|
|
- extendParam.YawOfCicvLocation = data.Yaw
|
|
|
- extendParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
|
|
|
+ pjisuvParam.VelocityYOfCicvLocation = data.VelocityX
|
|
|
+ pjisuvParam.VelocityYOfCicvLocation = data.VelocityY
|
|
|
+ pjisuvParam.VelocityYOfCicvLocation = data.VelocityZ
|
|
|
+ pjisuvParam.YawOfCicvLocation = data.Yaw
|
|
|
+ pjisuvParam.AngularVelocityZOfCicvLocation = data.AngularVelocityZ
|
|
|
+ pjisuvParam.PositionXOfCicvLocation = data.PositionX
|
|
|
+ pjisuvParam.PositionYOfCicvLocation = data.PositionY
|
|
|
}
|
|
|
mutexOfCicvLocation.RUnlock()
|
|
|
|
|
|
@@ -646,11 +651,11 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
// 更新共享变量
|
|
|
mutexOfPjControlPub.RLock()
|
|
|
{
|
|
|
- extendParam.NumCountPjiControlCommandOfPjControlPub++
|
|
|
- if extendParam.NumCountPjiControlCommandOfPjControlPub == 10 {
|
|
|
- extendParam.EgoSteeringCmdOfPjControlPub = append(extendParam.EgoSteeringCmdOfPjControlPub, data.ICPVCmdStrAngle)
|
|
|
- extendParam.EgoThrottleCmdOfPjControlPub = append(extendParam.EgoThrottleCmdOfPjControlPub, data.ICPVCmdAccPelPosAct)
|
|
|
- extendParam.NumCountPjiControlCommandOfPjControlPub = 0
|
|
|
+ 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()
|
|
|
@@ -877,10 +882,10 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
if obj.X <= 5 || math.Abs(float64(obj.Y)) >= 10 {
|
|
|
continue
|
|
|
}
|
|
|
- if _, ok := extendParam.ObjDicOfTpperception[obj.Id]; !ok {
|
|
|
- extendParam.ObjDicOfTpperception[obj.Id] = []float32{}
|
|
|
+ if _, ok := pjisuvParam.ObjDicOfTpperception[obj.Id]; !ok {
|
|
|
+ pjisuvParam.ObjDicOfTpperception[obj.Id] = []float32{}
|
|
|
}
|
|
|
- extendParam.ObjDicOfTpperception[obj.Id] = append(extendParam.ObjDicOfTpperception[obj.Id], obj.Y)
|
|
|
+ pjisuvParam.ObjDicOfTpperception[obj.Id] = append(pjisuvParam.ObjDicOfTpperception[obj.Id], obj.Y)
|
|
|
}
|
|
|
}
|
|
|
mutexOfTpperception.RUnlock()
|
|
|
@@ -891,7 +896,7 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
var faultLabel string
|
|
|
for _, f := range masterConfig.RuleOfTpperception {
|
|
|
- faultLabel = f(data, extendParam)
|
|
|
+ faultLabel = f(data, pjisuvParam)
|
|
|
if faultLabel != "" {
|
|
|
saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
break
|
|
|
@@ -1047,13 +1052,13 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
// 更新共享变量
|
|
|
mutexOfDataRead.RLock()
|
|
|
{
|
|
|
- extendParam.NumCountDataReadOfDataRead++
|
|
|
- if extendParam.NumCountDataReadOfDataRead == 10 {
|
|
|
- extendParam.EgoSteeringRealOfDataRead = append(extendParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
|
|
|
- extendParam.EgoThrottleRealOfDataRead = append(extendParam.EgoThrottleRealOfDataRead, data.AccPed2)
|
|
|
- extendParam.NumCountDataReadOfDataRead = 0
|
|
|
+ pjisuvParam.NumCountDataReadOfDataRead++
|
|
|
+ if pjisuvParam.NumCountDataReadOfDataRead == 10 {
|
|
|
+ pjisuvParam.EgoSteeringRealOfDataRead = append(pjisuvParam.EgoSteeringRealOfDataRead, data.ActStrWhAng)
|
|
|
+ pjisuvParam.EgoThrottleRealOfDataRead = append(pjisuvParam.EgoThrottleRealOfDataRead, data.AccPed2)
|
|
|
+ pjisuvParam.NumCountDataReadOfDataRead = 0
|
|
|
}
|
|
|
- extendParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
|
|
|
+ pjisuvParam.StrgAngleRealValueOfDataRead = data.ActStrWhAng
|
|
|
}
|
|
|
mutexOfDataRead.RUnlock()
|
|
|
subscribersTimeMutexes[i].Lock()
|
|
|
@@ -1103,6 +1108,39 @@ func PrepareTimeWindowProducerQueue() {
|
|
|
})
|
|
|
}
|
|
|
|
|
|
+ // 39
|
|
|
+ if topic == masterConfig.TopicOfPjVehicleFdbPub && len(masterConfig.RuleOfPjVehicleFdbPub) > 0 {
|
|
|
+ subscribers[i], err = goroslib.NewSubscriber(goroslib.SubscriberConf{
|
|
|
+ Node: commonConfig.RosNode,
|
|
|
+ Topic: topic,
|
|
|
+ Callback: func(data *pjisuv_msgs.VehicleFdb) {
|
|
|
+ // 更新共享变量
|
|
|
+ mutexOfPjControlPub.RLock()
|
|
|
+ {
|
|
|
+ pjisuvParam.AutomodeOfPjVehicleFdbPub = data.Automode
|
|
|
+ }
|
|
|
+ mutexOfPjControlPub.RUnlock()
|
|
|
+ subscribersTimeMutexes[i].Lock()
|
|
|
+ if time.Since(subscribersTimes[i]).Seconds() > 1 {
|
|
|
+ subscribersMutexes[i].Lock()
|
|
|
+ faultHappenTime := util.GetNowTimeCustom() // 获取当前故障发生时间
|
|
|
+ lastTimeWindow := commonEntity.GetLastTimeWindow() // 获取最后一个时间窗口
|
|
|
+ var faultLabel string
|
|
|
+ for _, f := range masterConfig.RuleOfPjVehicleFdbPub {
|
|
|
+ faultLabel = f(data, pjisuvParam)
|
|
|
+ if faultLabel != "" {
|
|
|
+ saveTimeWindow(faultLabel, faultHappenTime, lastTimeWindow)
|
|
|
+ subscribersTimes[i] = time.Now()
|
|
|
+ break
|
|
|
+ }
|
|
|
+ }
|
|
|
+ subscribersMutexes[i].Unlock()
|
|
|
+ }
|
|
|
+ subscribersTimeMutexes[i].Unlock()
|
|
|
+ },
|
|
|
+ })
|
|
|
+ }
|
|
|
+
|
|
|
if err != nil {
|
|
|
c_log.GlobalLogger.Info("创建订阅者报错：", err)
|
|
|
//TODO 如何回传日志
|
