joylink/rts-sim-testing-service
6
0
Fork 0
Code Issues 8 Pull Requests Actions Packages Projects Releases Wiki Activity

接收动力学数据的处理函数中增加处理车头方向逻辑

Browse Source
This commit is contained in:
joylink_zhangsai 2023-08-24 18:22:25 +08:00
parent 945ae4a43b
commit f2dbef4340
8 changed files with 294 additions and 95 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
ats/calculate/calculate.go Normal file
View File

@ -0,0 +1,162 @@
package calculate
import (
"errors"
"fmt"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/face"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/model/device"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/model/ref"
"math"
"strconv"
)
// CalculateLinkPositionAndDirection 根据起始link位置、偏移量、偏移方向，计算偏移后的位置及方向。偏移方向上的剩余距离不够，会返回error
func CalculateLinkPositionAndDirection(simulation *memory.VerifySimulation, linkIndex int32, linkOffset int32, up bool,
offset int32) (int32, int32, bool, error) {
vs := memory.QueryMapVerifyStructure(simulation.MapId)
direction := "下"
if up {
direction = "上"
}
var tempOffset int32
if up {
tempOffset = linkOffset + offset
linkModel := vs.LinkModelMap[linkIndex]
if tempOffset <= linkModel.Length {
return linkIndex, tempOffset, up, nil
}
if linkModel.BRelatedSwitchRef == nil {
return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
}
turnoutState := memory.GetTurnoutState(simulation, linkModel.ARelatedSwitchRef.SwitchDevice.GetIndex())
turnoutModel := linkModel.ARelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
var linkPort *ref.DevicePort[*device.LinkModel]
if turnoutState.Normal {
switch linkModel.BRelatedSwitchRef.Port {
case face.A:
linkPort = turnoutModel.BLinkPort
case face.B:
linkPort = turnoutModel.ALinkPort
}
} else if turnoutState.Reverse {
switch linkModel.BRelatedSwitchRef.Port {
case face.A:
linkPort = turnoutModel.CLinkPort
case face.C:
linkPort = turnoutModel.ALinkPort
}
}
if linkPort != nil {
var newOffset int32
var newUp bool
i, _ := strconv.Atoi(linkPort.Device.Index)
switch linkPort.Port {
case face.A:
newOffset = 0
newUp = true
case face.B:
newOffset = linkPort.Device.Length
newUp = false
}
return CalculateLinkPositionAndDirection(simulation, int32(i), newOffset, newUp, tempOffset-linkModel.Length)
} else {
return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
}
} else {
tempOffset = linkOffset - offset
if tempOffset >= 0 {
return linkIndex, tempOffset, up, nil
}
linkModel := vs.LinkModelMap[linkIndex]
if linkModel.ARelatedSwitchRef == nil {
return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
}
turnoutState := memory.GetTurnoutState(simulation, linkModel.ARelatedSwitchRef.SwitchDevice.GetIndex())
turnoutModel := linkModel.ARelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
var linkPort *ref.DevicePort[*device.LinkModel]
if turnoutState.Normal {
switch linkModel.ARelatedSwitchRef.Port {
case face.A:
linkPort = turnoutModel.BLinkPort
case face.B:
linkPort = turnoutModel.ALinkPort
}
} else if turnoutState.Reverse {
switch linkModel.ARelatedSwitchRef.Port {
case face.A:
linkPort = turnoutModel.CLinkPort
case face.C:
linkPort = turnoutModel.ALinkPort
}
}
if linkPort != nil {
var newOffset int32
var newUp bool
i, _ := strconv.Atoi(linkPort.Device.Index)
switch linkPort.Port {
case face.A:
newOffset = 0
newUp = true
case face.B:
newOffset = linkPort.Device.Length
newUp = false
}
return CalculateLinkPositionAndDirection(simulation, int32(i), newOffset, newUp, int32(math.Abs(float64(tempOffset))))
} else {
return linkIndex, 0, up, errors.New(fmt.Sprintf("link位置[%d-%d]向[%s]偏移[%d]超出极限", linkIndex, linkOffset, direction, offset))
}
}
}
// CalculateLen 计算两个link位置的间距
// @param up 优先向这个方向查找终点
func CalculateLen(start *ref.DevicePosition[*device.LinkModel], end *ref.DevicePosition[*device.LinkModel], up bool) (int, error) {
return calculateLen(start, end, up, 0)
}
//// CalculateDirection 计算以{link}为基准的dp方向
//// @param up 优先向这个方向查找link
//func CalculateDirection(dp *ref.DevicePort[*device.LinkModel], link *device.LinkModel, up bool) *ref.DevicePort[*device.LinkModel] {
// if link == dp.Device {
// return dp
// }
//
//}
func calculateLen(start *ref.DevicePosition[*device.LinkModel], end *ref.DevicePosition[*device.LinkModel],
up bool, l int) (int, error) {
if start.Device == end.Device {
return l + int(math.Abs(float64(start.Offset-end.Offset))), nil
}
var turnoutPort *ref.SwitchRef
if up {
l += int(start.Device.Length - start.Offset)
turnoutPort = start.Device.BRelatedSwitchRef
} else {
l += int(start.Offset)
turnoutPort = start.Device.ARelatedSwitchRef
}
var lps []*ref.DevicePosition[*device.LinkModel]
if turnoutPort != nil {
turnoutModel := start.Device.BRelatedSwitchRef.SwitchDevice.(*device.SwitchDeviceModel)
ref.ConvertFromDevicePort(turnoutModel.BLinkPort)
switch turnoutPort.Port {
case face.A:
lps = append(lps, ref.ConvertFromDevicePort(turnoutModel.BLinkPort))
lps = append(lps, ref.ConvertFromDevicePort(turnoutModel.CLinkPort))
case face.B | face.C:
lps = append(lps, ref.ConvertFromDevicePort(turnoutModel.ALinkPort))
}
}
for _, lp := range lps {
i, err := calculateLen(lp, end, lp.Offset == 0, l)
if err == nil {
return i, nil
}
}
return 0, errors.New(fmt.Sprintf("从[%s]到[%s]的间距无法计算", start.String(), end.String()))
}

108
ats/verify/simulation/simulation_manage.go
View File

@ -70,10 +70,8 @@ func init() {
// 给半实物仿真发送速度
zap.S().Info("发送到vobc发送的速度", info.Speed*36)
vobc.SendTrainSpeedTask(info.Speed * 36)
// 处理列车车头方向
handlerTrainHeadDirection(simulation, info, trainState)
// 更新列车状态
memory.UpdateTrainState(simulation, convert(info, trainState, memory.QueryMapVerifyStructure(simulation.MapId)))
memory.UpdateTrainState(simulation, convert(info, trainState, simulation))
break
}
}
@ -151,11 +149,13 @@ func GetSimulationArr() []*memory.VerifySimulation {
return result
}
func convert(info *dynamics.TrainInfo, sta *state.TrainState, vs *memory.VerifyStructure) *state.TrainState {
func convert(info *dynamics.TrainInfo, sta *state.TrainState, simulation *memory.VerifySimulation) *state.TrainState {
vs := memory.QueryMapVerifyStructure(simulation.MapId)
zap.S().Debugf("原始消息：[%d-%d-%d]", info.Number, info.Link, info.LinkOffset)
sta.HeadLinkId = strconv.Itoa(int(info.Link))
sta.HeadLinkOffset = int64(info.LinkOffset)
sta.Up = info.Up
//sta.Up = info.Up
id, port, offset, runDirection, pointTo := memory.QueryDeviceByCalcLink(vs, int32(info.Link), sta.HeadLinkOffset, sta.Up, sta.RunDirection)
zap.S().Debugf("转换后的消息：[%d-车头:%s-偏移:%d-上行:%v-是否ab:%v]", info.Number, id, offset, runDirection, pointTo)
sta.HeadDeviceId = id
@ -163,36 +163,6 @@ func convert(info *dynamics.TrainInfo, sta *state.TrainState, vs *memory.VerifyS
sta.HeadOffset = offset
sta.PointTo = pointTo
sta.RunDirection = runDirection
/*
modeller := vs.LinkModelMap[int32(info.Link)]
model := modeller.(*device.LinkModel)
for i, dp := range model.DevicePositions {
if uint32(dp.Offset) >= info.LinkOffset {
var minLinkRef *graphicData.RelatedRef
var minOffset int32
var maxOffset int32
if i == 0 {
minLinkRef = model.SectionLinkMap[dp.Device.GetGraphicId()]
minOffset = dp.Offset
maxOffset = model.DevicePositions[i+1].Offset
} else {
minLinkRef = model.SectionLinkMap[model.DevicePositions[i-1].Device.GetGraphicId()]
minOffset = model.DevicePositions[i-1].Offset
maxOffset = dp.Offset
}
switch minLinkRef.DevicePort {
case 0:
sta.HeadLinkId = minLinkRef.GetId()
sta.HeadLinkOffset = int64(info.LinkOffset - uint32(minOffset))
case 1:
sta.HeadLinkId = minLinkRef.GetId()
sta.HeadLinkOffset = int64(uint32(maxOffset) - info.LinkOffset)
}
break
}
}
zap.S().Debugf("转换后的消息：[%d-%s-%d]", info.Number, sta.HeadLinkId, sta.HeadLinkOffset)
*/
sta.Slope = int32(info.Slope)
sta.Upslope = info.UpSlope
sta.RunningUp = info.Up
@ -207,6 +177,17 @@ func convert(info *dynamics.TrainInfo, sta *state.TrainState, vs *memory.VerifyS
sta.TailSensorSpeed2 = info.TailSpeed2 * 3.6
sta.HeadRadarSpeed = info.HeadRadarSpeed * 3.6
sta.TailRadarSpeed = info.TailRadarSpeed * 3.6
//判定车头方向
v, ok := simulation.Memory.Status.VobcTrainStateMap.Load(sta.Id)
if ok {
vobcState := v.(*state.TrainVobcState)
if vobcState.DirectionForward {
sta.HeadDirection = info.Up
} else if vobcState.DirectionBackward {
sta.HeadDirection = !info.Up
}
}
return sta
}
@ -253,28 +234,28 @@ func buildLineBaseInfo(vs *memory.VerifyStructure) *dynamics.LineBaseInfo {
BRelTurnoutPoint: link.BRelatedSwitchRef.Port.Name(),
})
}
for _, slope := range vs.SlopeModelMap {
id, _ := strconv.Atoi(slope.Index)
slopes = append(slopes, &dynamics.Slope{
ID: int32(id),
StartLinkId: slope.StartLinkIndex,
StartLinkOffset: slope.StartLinkOffset,
EndLinkId: slope.EndLinkIndex,
EndLinkOffset: slope.EndLinkOffset,
DegreeTrig: slope.DegreeTrig,
})
}
for _, curve := range vs.CurveModelMap {
id, _ := strconv.Atoi(curve.Index)
curves = append(curves, &dynamics.Curve{
ID: int32(id),
StartLinkId: curve.StartLinkIndex,
StartLinkOffset: curve.StartLinkOffset,
EndLinkId: curve.EndLinkIndex,
EndLinkOffset: curve.EndLinkOffset,
Curvature: curve.Curvature,
})
}
//for _, slope := range vs.SlopeModelMap {
// id, _ := strconv.Atoi(slope.Index)
// slopes = append(slopes, &dynamics.Slope{
// ID: int32(id),
// StartLinkId: slope.StartLinkIndex,
// StartLinkOffset: slope.StartLinkOffset,
// EndLinkId: slope.EndLinkIndex,
// EndLinkOffset: slope.EndLinkOffset,
// DegreeTrig: slope.DegreeTrig,
// })
//}
//for _, curve := range vs.CurveModelMap {
// id, _ := strconv.Atoi(curve.Index)
// curves = append(curves, &dynamics.Curve{
// ID: int32(id),
// StartLinkId: curve.StartLinkIndex,
// StartLinkOffset: curve.StartLinkOffset,
// EndLinkId: curve.EndLinkIndex,
// EndLinkOffset: curve.EndLinkOffset,
// Curvature: curve.Curvature,
// })
//}
return &dynamics.LineBaseInfo{
LinkList: links,
SlopeList: slopes,
@ -314,16 +295,3 @@ func decoderVobcTrainState(buf []byte) *state.TrainVobcState {
trainVobcInfo.AllDoorClose = (b4 & (1 << 3)) != 0
return trainVobcInfo
}
// 处理列车车头逻辑
func handlerTrainHeadDirection(simulation *memory.VerifySimulation, info *dynamics.TrainInfo, status *state.TrainState) {
v, ok := simulation.Memory.Status.VobcTrainStateMap.Load(status.Id)
if ok {
vobcState := v.(*state.TrainVobcState)
if (vobcState.Tc1Active || vobcState.Tc2Active) && (vobcState.DirectionBackward || vobcState.DirectionForward) { // 有激活端
// info.Up 运行link方向
//vobcState.DirectionForward
}
}
}

62
ats/verify/simulation/wayside/memory/wayside_memory_map.go
View File

@ -61,15 +61,15 @@ type GraphicInfoMapStructure struct {
//key-index
CalcLinkMap map[int32]*graphicData.CalculateLink
//设备在link上的位置。key-设备id
DevicePositionMap map[string]*ref.DevicePosition
DevicePositionMap map[string]*ref.DevicePosition[*device.LinkModel]
}
// 计算link、物理区段、道岔相互转换时用到的结构体
type calcLinkPositionStruct struct {
index string // 地图元素的Index
deviceType int // 1 计轴， 2 道岔
kilometer *graphicData.KilometerSystem // 元素对应的公里标
position *ref.DevicePosition // 元素在link上对应的偏移量
index string // 地图元素的Index
deviceType int // 1 计轴， 2 道岔
kilometer *graphicData.KilometerSystem // 元素对应的公里标
position *ref.DevicePosition[face.DeviceModeller] // 元素在link上对应的偏移量
}
// 将发布的地图数据放入内存中
@ -98,7 +98,7 @@ func PublishMapVerifyStructure(graphic *model.PublishedGi) *VerifyStructure {
LogicSectionMap: make(map[string]*graphicData.LogicSection),
SignalMap: make(map[string]*graphicData.Signal),
CalcLinkMap: make(map[int32]*graphicData.CalculateLink),
DevicePositionMap: make(map[string]*ref.DevicePosition),
DevicePositionMap: make(map[string]*ref.DevicePosition[*device.LinkModel]),
}
graphicStorage := &graphicData.RtssGraphicStorage{}
proto.Unmarshal(graphic.Proto, graphicStorage)
@ -149,7 +149,7 @@ func sectionMapToLink(vm *VerifyStructure, id string, offset int64, runDirection
// 确定link信息
var linkId int32
// 获取计轴信息
axlePointPositionMap := make(map[string]*ref.DevicePosition)
axlePointPositionMap := make(map[string]*ref.DevicePosition[face.DeviceModeller])
for k, lm := range vm.LinkModelMap {
for _, p := range lm.DevicePositions {
_, ok := p.Device.(*device.AxlePointDeviceModel)
@ -176,7 +176,7 @@ func sectionMapToLink(vm *VerifyStructure, id string, offset int64, runDirection
abDirection := pointB == nil
ak := (pointA.(*device.AxlePointDeviceModel)).KilometerSystem // 获取a点公里标
ao, bo := ap.Offset, int32(math.MaxInt32) // a，b的偏移量，默认B点比A点大
var bp *ref.DevicePosition // 定义b点偏移对象
var bp *ref.DevicePosition[face.DeviceModeller] // 定义b点偏移对象
var bk *graphicData.KilometerSystem // 定义b点公里标
if !abDirection && axlePointPositionMap[pointB.GetIndex()] != nil {
bp = axlePointPositionMap[pointB.GetIndex()]
@ -464,31 +464,61 @@ func initLinks(links []*graphicData.CalculateLink, vs *VerifyStructure, dataMap
Length: link.Length,
}
if link.ARelatedRef != nil {
linkModel.ARelatedSwitchRef = ref.SwitchRef{
SwitchDevice: deviceIdMap[link.ARelatedRef.Id],
Port: face.GetPortEnum(int8(link.ARelatedRef.DevicePort)),
modeller := deviceIdMap[link.ARelatedRef.Id]
port := face.GetPortEnum(int8(link.ARelatedRef.DevicePort))
linkModel.ARelatedSwitchRef = &ref.SwitchRef{
SwitchDevice: modeller,
Port: port,
}
turnoutModel := modeller.(*device.SwitchDeviceModel)
linkPort := &ref.DevicePort[*device.LinkModel]{
Device: linkModel,
Port: face.A,
}
switch port {
case face.A:
turnoutModel.ALinkPort = linkPort
case face.B:
turnoutModel.BLinkPort = linkPort
case face.C:
turnoutModel.CLinkPort = linkPort
}
}
if link.BRelatedRef != nil {
linkModel.BRelatedSwitchRef = ref.SwitchRef{
modeller := deviceIdMap[link.ARelatedRef.Id]
port := face.GetPortEnum(int8(link.ARelatedRef.DevicePort))
linkModel.BRelatedSwitchRef = &ref.SwitchRef{
SwitchDevice: deviceIdMap[link.BRelatedRef.Id],
Port: face.GetPortEnum(int8(link.BRelatedRef.DevicePort)),
}
turnoutModel := modeller.(*device.SwitchDeviceModel)
linkPort := &ref.DevicePort[*device.LinkModel]{
Device: linkModel,
Port: face.B,
}
switch port {
case face.A:
turnoutModel.ALinkPort = linkPort
case face.B:
turnoutModel.BLinkPort = linkPort
case face.C:
turnoutModel.CLinkPort = linkPort
}
}
vs.LinkModelMap[link.Index] = linkModel
dataMap.CalcLinkMap[link.Index] = link
//构建DevicePosition（DP）切片
var dps []*ref.DevicePosition
var dps []*ref.DevicePosition[face.DeviceModeller]
for _, dp := range link.DevicePositions {
modeller := deviceIdMap[dp.DeviceId]
if modeller == nil {
continue
}
dps = append(dps, &ref.DevicePosition{
dps = append(dps, &ref.DevicePosition[face.DeviceModeller]{
Device: modeller,
Offset: dp.Offset,
})
dataMap.DevicePositionMap[dp.DeviceId] = &ref.DevicePosition{
dataMap.DevicePositionMap[dp.DeviceId] = &ref.DevicePosition[*device.LinkModel]{
Device: linkModel,
Offset: dp.Offset,
}
@ -919,7 +949,7 @@ func convertPositionToCalcPosition(link *device.LinkModel) []*calcLinkPositionSt
}
// 判断上下行与link方向是否一直
func convertRunDirectionToUp(ak, bk *graphicData.KilometerSystem, ap, bp *ref.DevicePosition, direction bool) bool {
func convertRunDirectionToUp(ak, bk *graphicData.KilometerSystem, ap, bp *ref.DevicePosition[face.DeviceModeller], direction bool) bool {
if bk == nil {
return direction
}

5
ats/verify/simulation/wayside/memory/wayside_memory_turnout.go
View File

@ -54,3 +54,8 @@ func GetChangeTurnoutState(simulation *VerifySimulation) []*state.SwitchState {
}
return switchArr
}
func GetTurnoutState(simulation *VerifySimulation, turnoutId string) *state.SwitchState {
v, _ := simulation.Memory.Status.SwitchStateMap.Load(turnoutId)
return v.(*state.SwitchState)
}

6
ats/verify/simulation/wayside/model/device/link_device_model.go
View File

@ -10,9 +10,9 @@ type LinkModel struct {
//长度
Length int32
//A端连接的道岔端点
ARelatedSwitchRef ref.SwitchRef
ARelatedSwitchRef *ref.SwitchRef
//B端连接的道岔端点
BRelatedSwitchRef ref.SwitchRef
BRelatedSwitchRef *ref.SwitchRef
//Link上的设备及位置(包括A、B端的设备)(按offset排序)
DevicePositions []*ref.DevicePosition
DevicePositions []*ref.DevicePosition[face.DeviceModeller]
}

12
ats/verify/simulation/wayside/model/device/switch_device_model.go
View File

@ -13,14 +13,20 @@ type SwitchDeviceModel struct {
//端点的公里标,单位为mm
//list中元素类型为graphicData.KilometerSystem
KilometerSystems []*graphicData.KilometerSystem
//道岔A端连接的link
//道岔A端连接的sectionLink
LinkRefA *ref.LinkRef
//道岔B端连接的link
//道岔B端连接的sectionLink
LinkRefB *ref.LinkRef
//道岔C端连接的link
//道岔C端连接的sectionLink
LinkRefC *ref.LinkRef
// 道岔相连的计轴信息
AxlePoints map[string]*AxlePointDeviceModel
// A端连接的Link端点
ALinkPort *ref.DevicePort[*LinkModel]
// B端连接的Link端点
BLinkPort *ref.DevicePort[*LinkModel]
// C端连接的Link端点
CLinkPort *ref.DevicePort[*LinkModel]
}
// 获取道岔端口关联的轨道

9
ats/verify/simulation/wayside/model/ref/device_port.go Normal file
View File

@ -0,0 +1,9 @@
package ref
import "joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/face"
// DevicePort 两个作用，一是表示设备的端点；而是表示以此设备为基准的方向
type DevicePort[T face.DeviceModeller] struct {
Device T
Port face.PortEnum
}

25
ats/verify/simulation/wayside/model/ref/device_position.go
View File

@ -1,8 +1,27 @@
package ref
import "joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/face"
import (
"fmt"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/face"
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/model/device"
)
type DevicePosition struct {
Device face.DeviceModeller
type DevicePosition[T face.DeviceModeller] struct {
Device T
Offset int32
}
func (dp *DevicePosition[T]) String() string {
return fmt.Sprintf("%s-%d", dp.Device.GetIndex(), dp.Offset)
}
func ConvertFromDevicePort(dp *DevicePort[*device.LinkModel]) *DevicePosition[*device.LinkModel] {
var offset int32 = 0
if dp.Port == face.B {
offset = dp.Device.Length
}
return &DevicePosition[*device.LinkModel]{
Device: dp.Device,
Offset: offset,
}
}