diff --git a/bj-rtss-message b/bj-rtss-message index fe2d65a..fba9d69 160000 --- a/bj-rtss-message +++ b/bj-rtss-message @@ -1 +1 @@ -Subproject commit fe2d65a91d08c62c55608aa28f799bfaaf18808e +Subproject commit fba9d6979557d3b4ef2e3b3dbab1c907fcdd44f7 diff --git a/message_server/sfp_ms.go b/message_server/sfp_ms.go index ae3cf9f..fa75bee 100644 --- a/message_server/sfp_ms.go +++ b/message_server/sfp_ms.go @@ -347,31 +347,31 @@ func (ms *SfpMs) collectPlatformStates() ([]*state.PlatformState, error) { sta.SpksState = append(sta.SpksState, &state.ReplyState{Code: "SPKS4", Xh: getRelayXqVal(spkElectronic.SPKSS4J)}) } } - if platformScreenDoorMap[platform.Common.Id] != nil { - for _, psdId := range platformScreenDoorMap[platform.Common.Id] { - psdUid, ok := uidsMap.PsdIds[psdId] - if !ok { - continue - } - psdEntry, ok := entity.GetEntityByUid(ms.vs.World, psdUid.Uid) - if !ok { - return nil, fmt.Errorf("屏蔽门实体不存在: World id=%d, uid=%s", ms.vs.World.Id(), psdUid.Uid) - } - if psdEntry.HasComponent(component.PlatformMkxCircuitType) { - mkxCircuit := component.PlatformMkxCircuitType.Get(psdEntry) - mkxj := &state.MkxJState{Code: psdUid.Code} - if mkxCircuit.PABJ != nil { - mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "PABJ", Xh: getRelayXqVal(mkxCircuit.PABJ)}) - } - if mkxCircuit.PCBJ != nil { - mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "PCBJ", Xh: getRelayXqVal(mkxCircuit.PCBJ)}) - } - if mkxCircuit.POBJ != nil { - mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "POBJ", Xh: getRelayXqVal(mkxCircuit.POBJ)}) - } - sta.MkxJState = append(sta.MkxJState, mkxj) - } + psdId := platformScreenDoorMap[platform.Common.Id] + if psdId != "" { + psdUid, ok := uidsMap.PsdIds[psdId] + if !ok { + continue } + psdEntry, ok := entity.GetEntityByUid(ms.vs.World, psdUid.Uid) + if !ok { + return nil, fmt.Errorf("屏蔽门实体不存在: World id=%d, uid=%s", ms.vs.World.Id(), psdUid.Uid) + } + if psdEntry.HasComponent(component.PlatformMkxCircuitType) { + mkxCircuit := component.PlatformMkxCircuitType.Get(psdEntry) + mkxj := &state.MkxJState{Code: psdUid.Code} + if mkxCircuit.PABJ != nil { + mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "站台确认继电器", Xh: getRelayXqVal(mkxCircuit.PABJ)}) + } + if mkxCircuit.PCBJ != nil { + mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "站台关门继电器", Xh: getRelayXqVal(mkxCircuit.PCBJ)}) + } + if mkxCircuit.POBJ != nil { + mkxj.ReplyState = append(mkxj.ReplyState, &state.ReplyState{Code: "站台开门继电器", Xh: getRelayXqVal(mkxCircuit.POBJ)}) + } + sta.MkxJState = mkxj + } + } states = append(states, sta) } @@ -379,10 +379,10 @@ func (ms *SfpMs) collectPlatformStates() ([]*state.PlatformState, error) { } // 将屏蔽门关联到站台 -func wrapScreenDoorToPlatform(mapData *graphicData.RtssGraphicStorage) map[string][]string { - platformMap := make(map[string][]string, len(mapData.Platforms)) +func wrapScreenDoorToPlatform(mapData *graphicData.RtssGraphicStorage) map[string]string { + platformMap := make(map[string]string, len(mapData.Platforms)) for _, s := range mapData.ScreenDoors { - platformMap[s.RefPlatformId] = append(platformMap[s.RefPlatformId], s.Common.Id) + platformMap[s.RefPlatformId] = s.Common.Id } return platformMap } diff --git a/ts/protos/state/device_state.pb.go b/ts/protos/state/device_state.pb.go index da60d0c..2dc3596 100644 --- a/ts/protos/state/device_state.pb.go +++ b/ts/protos/state/device_state.pb.go @@ -621,7 +621,7 @@ type PlatformState struct { // SPKS继电器状态 SpksState []*ReplyState `protobuf:"bytes,3,rep,name=spksState,proto3" json:"spksState,omitempty"` // 门控箱继电器状态 - MkxJState []*MkxJState `protobuf:"bytes,4,rep,name=mkxJState,proto3" json:"mkxJState,omitempty"` + MkxJState *MkxJState `protobuf:"bytes,4,opt,name=mkxJState,proto3" json:"mkxJState,omitempty"` } func (x *PlatformState) Reset() { @@ -677,7 +677,7 @@ func (x *PlatformState) GetSpksState() []*ReplyState { return nil } -func (x *PlatformState) GetMkxJState() []*MkxJState { +func (x *PlatformState) GetMkxJState() *MkxJState { if x != nil { return x.MkxJState } @@ -2327,7 +2327,7 @@ var file_device_state_proto_rawDesc = []byte{ 0x61, 0x74, 0x65, 0x18, 0x03, 0x20, 0x03, 0x28, 0x0b, 0x32, 0x11, 0x2e, 0x73, 0x74, 0x61, 0x74, 0x65, 0x2e, 0x52, 0x65, 0x70, 0x6c, 0x79, 0x53, 0x74, 0x61, 0x74, 0x65, 0x52, 0x09, 0x73, 0x70, 0x6b, 0x73, 0x53, 0x74, 0x61, 0x74, 0x65, 0x12, 0x2e, 0x0a, 0x09, 0x6d, 0x6b, 0x78, 0x4a, 0x53, - 0x74, 0x61, 0x74, 0x65, 0x18, 0x04, 0x20, 0x03, 0x28, 0x0b, 0x32, 0x10, 0x2e, 0x73, 0x74, 0x61, + 0x74, 0x61, 0x74, 0x65, 0x18, 0x04, 0x20, 0x01, 0x28, 0x0b, 0x32, 0x10, 0x2e, 0x73, 0x74, 0x61, 0x74, 0x65, 0x2e, 0x4d, 0x6b, 0x78, 0x4a, 0x53, 0x74, 0x61, 0x74, 0x65, 0x52, 0x09, 0x6d, 0x6b, 0x78, 0x4a, 0x53, 0x74, 0x61, 0x74, 0x65, 0x22, 0x1e, 0x0a, 0x0c, 0x53, 0x74, 0x61, 0x74, 0x69, 0x6f, 0x6e, 0x53, 0x74, 0x61, 0x74, 0x65, 0x12, 0x0e, 0x0a, 0x02, 0x69, 0x64, 0x18, 0x01, 0x20, diff --git a/ts/simulation/wayside/memory/wayside_memory_map.go b/ts/simulation/wayside/memory/wayside_memory_map.go index 4cc4b5e..ea9633b 100644 --- a/ts/simulation/wayside/memory/wayside_memory_map.go +++ b/ts/simulation/wayside/memory/wayside_memory_map.go @@ -2,12 +2,12 @@ package memory import ( "fmt" - "math" "sort" - "strconv" "strings" "sync" + "joylink.club/rtsssimulation/component" + "joylink.club/rtsssimulation/entity" "joylink.club/rtsssimulation/repository" proto2 "joylink.club/rtsssimulation/repository/model/proto" @@ -17,7 +17,6 @@ import ( "joylink.club/bj-rtsts-server/dto" "joylink.club/bj-rtsts-server/sys_error" "joylink.club/bj-rtsts-server/ts/protos/graphicData" - "joylink.club/bj-rtsts-server/ts/protos/state" ) var ( @@ -96,18 +95,6 @@ func QueryGiId(name string) int32 { return value.(int32) } -// 根据区段，道岔偏移量返回linkID和link相对偏移量 -func QueryEcsLinkByDeviceInfo(repo *repository.Repository, mapId int32, status *state.TrainState) (int32, int64, bool, bool, int64) { - id := status.HeadDeviceId - if status.DevicePort == "" { - uid := QueryUidByMidAndComId(mapId, id, &graphicData.Section{}) - return sectionMapToEcsLink(repo, uid, status) - } else { - uid := QueryUidByMidAndComId(mapId, id, &graphicData.Turnout{}) - return turnoutMapToEcsLink(repo, uid, status) - } -} - func GetStorageIBPMapData(mapCode string) *graphicData.IBPGraphicStorage { // 处理关联的IBP盘信息 if mapCode == "" { @@ -124,116 +111,39 @@ func GetStorageIBPMapData(mapCode string) *graphicData.IBPGraphicStorage { return ibpMapData.(*graphicData.IBPGraphicStorage) } -// 根据物理区段上的偏移量（基于区段A端），找到所在link的linkId与偏移量 -func sectionMapToEcsLink(repo *repository.Repository, id string, status *state.TrainState) (int32, int64, bool, bool, int64) { - runDirection := status.RunDirection - section := repo.FindPhysicalSection(id) - if section == nil { - panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", id))) - } - ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() - // 检查区段长度是否足够放下列车 - status.HeadOffset = checkDeviceContainTrain(status.HeadOffset, status.TrainLength, bo-ao, id) - link := section.ALinkPosition().Link() - // 是否从A到B，统一坐标 - ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer()) - akv, bkv := ak.Value, bk.Value - // 上行 - var up, abDirection bool - if runDirection { - abDirection = akv < bkv - if abDirection { - up = ao < bo - } else { - up = ao > bo - } - } else { - abDirection = akv > bkv - if abDirection { - up = ao < bo - } else { - up = ao > bo - } - } - linkId, _ := strconv.Atoi(link.Identity.Id()) - trainKilometer := concertTrainKilometer(akv, status.HeadOffset, up) - if ao < bo { - return int32(linkId), ao + status.HeadOffset, up, abDirection, trainKilometer - } else { - return int32(linkId), ao - status.HeadOffset, up, abDirection, trainKilometer - } -} - -// 根据道岔上的偏移量（基于岔心位置），找到所在link的linkId与偏移量 -func turnoutMapToEcsLink(repo *repository.Repository, id string, status *state.TrainState) (int32, int64, bool, bool, int64) { - port, runDirection := status.DevicePort, status.RunDirection - turnout := repo.FindTurnout(id) - if turnout == nil { - panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", id))) - } - var portPosition *repository.LinkPosition - var crossKm, portKm *proto2.Kilometer - switch port { - case "A": - portPosition = turnout.FindLinkPositionByPort(proto2.Port_A) - portKm = turnout.GetTurnoutKm(proto2.Port_A) - case "B": - portPosition = turnout.FindLinkPositionByPort(proto2.Port_B) - portKm = turnout.GetTurnoutKm(proto2.Port_B) - case "C": - portPosition = turnout.FindLinkPositionByPort(proto2.Port_C) - portKm = turnout.GetTurnoutKm(proto2.Port_C) - default: - panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port))) - } - // 岔心公里标 - crossKm = turnout.GetTurnoutKm(proto2.Port_None) - portKm, err := repo.ConvertKilometer(portKm, crossKm.CoordinateSystem) +// 转换成统一坐标公里标 +func convertRepoBaseKm(r *repository.Repository, km *proto2.Kilometer) *proto2.Kilometer { + k, err := r.ConvertKilometer(km, r.GetCoordinateInfo().Coordinate) if err != nil { - panic(sys_error.New("公里标转换出错", err)) - } - // 检查link偏移 - status.HeadOffset = checkDeviceContainTrain(status.HeadOffset, status.TrainLength, crossKm.Value-portKm.Value, id) - // 关联link - link := portPosition.Link() - isStart := link.ARelation().Device().Id() == id - up := runDirection - if (portKm.Value > crossKm.Value) != isStart { - up = !runDirection - } - pointTo := (portKm.Value > crossKm.Value) == runDirection - trainKilometer := concertTrainKilometer(crossKm.Value, status.HeadOffset, pointTo) - linkId, _ := strconv.Atoi(link.Identity.Id()) - if isStart { - return int32(linkId), status.HeadOffset, up, pointTo, trainKilometer - } else { - // 道岔长度 - turnoutLen := int64(math.Abs(float64(portKm.Value - crossKm.Value))) - return int32(linkId), portPosition.Offset() + turnoutLen - status.HeadOffset, up, pointTo, trainKilometer + panic(sys_error.New(fmt.Sprintf("公里标转换【%s->%s】错误", km.CoordinateSystem, r.GetCoordinateInfo().Coordinate))) } + return k } -// 根据linkID和link相对偏移量返回区段，道岔偏移量 -// 设备ID、端口、偏移量、上下行、AB走向 -func QueryDeviceByCalcLink(repo *repository.Repository, id string, offset int64, up bool) ( - deviceId, port string, deviceOffset int64, runDirection, pointTo bool, km int64) { - link := repo.FindLink(id) +// 根据传入link、偏移、link运行方向，查找所在设备信息 +// 入参：仿真、linkId、link偏移量、link运行方向 +// 输出：linkId、设备Id、设备端口、link偏移量、设备上偏移量、偏移量对应的公里标 +func CalcInitializeLink(sim *VerifySimulation, linkId string, offset int64, up bool) ( + outLinkId, deviceId, port string, outLinkOffset, deviceOffset int64, km *proto2.Kilometer) { + link := sim.Repo.FindLink(linkId) if link == nil { - panic(dto.ErrorDto{Code: dto.DataNotExist, Message: fmt.Sprintf("未找到link【%s】", id)}) - } - if offset > link.Length() { - panic(dto.ErrorDto{Code: dto.DataNotExist, Message: fmt.Sprintf("偏移【%d】超出link范围【%d】", offset, link.Length())}) + panic(sys_error.New(fmt.Sprintf("未找到link【%s】", linkId))) } + // 获取计算link与所在偏移 + outLinkId, outLinkOffset = findCalcLinkIdAndOffset(sim, link, offset) + calcLink := sim.Repo.FindLink(outLinkId) // 判断是否在道岔上 - onTurnout, isA := isOnLinkTurnout(link, offset) + onTurnout, isA := isOnLinkTurnout(calcLink, outLinkOffset) if onTurnout { - return ecsLinkMapToTurnout(repo, isA, offset, up, link) + deviceId, port, deviceOffset, km = calcTurnoutOffset(sim.Repo, calcLink, isA, outLinkOffset, up) } else { - return ecsLinkMapToSection(repo, offset, up, link) + deviceId, port, deviceOffset, km = calcSectionOffset(sim.Repo, calcLink, outLinkOffset, up) } + // 输出当前所在设备、端口、设备偏移 + return } -// 是否在link的道岔上 +// 列车是否在link的道岔上 func isOnLinkTurnout(link *repository.Link, offset int64) (bool, bool) { aTp := link.ARelation() var turnoutOffset int64 @@ -251,57 +161,123 @@ func isOnLinkTurnout(link *repository.Link, offset int64) (bool, bool) { return false, false } -// 处理在道岔上link映射 -func ecsLinkMapToTurnout(repo *repository.Repository, isA bool, offset int64, up bool, link *repository.Link) ( - deviceId, port string, deviceOffset int64, runDirection, pointTo bool, km int64) { +// 查找最终的linkId与link偏移 +// 入参：仿真、link、link偏移量 +// 输出：linkId、link偏移量 +func findCalcLinkIdAndOffset(sim *VerifySimulation, link *repository.Link, offset int64) (string, int64) { + if 0 <= offset && offset <= link.Length() { + return link.Id(), offset + } + // 超出端、超出长度 + tp, length := link.ARelation(), 0-offset + if offset > 0 { + tp, length = link.BRelation(), offset-link.Length() + } + if tp == nil { + panic(sys_error.New("列车偏移超出link位置")) + } + nextPort := getTurnoutNextPort(sim, tp) + if nextPort == proto2.Port_None { + panic(sys_error.New("列车偏移超出link位置")) + } + var nextLink *repository.Link + var isB bool + // 寻找匹配到的link + for _, l := range sim.Repo.LinkList() { + if l.ARelation() != nil && l.ARelation().Port() == nextPort && l.ARelation().Device().Id() == tp.Device().Id() { + nextLink = l + break + } + if l.BRelation() != nil && l.BRelation().Port() == nextPort && l.BRelation().Device().Id() == tp.Device().Id() { + nextLink = l + isB = true + break + } + } + // 没有找到连接信息，说明已经到尽头找不到位置 + if nextLink == nil { + panic(sys_error.New(fmt.Sprintf("未找到对应的link信息, linkId=%s, offset=%d", link.Id(), offset))) + } + // 下个link偏移 + nextOffset := length + if isB { + nextOffset = nextLink.Length() - length + } + return findCalcLinkIdAndOffset(sim, nextLink, nextOffset) +} + +// 根据当前link端，寻找下一个link连接端端口 +// 入参：仿真、道岔端口 +// 输出：道岔端口 +func getTurnoutNextPort(sim *VerifySimulation, tp *repository.TurnoutPort) proto2.Port { + entry, ok := entity.GetEntityByUid(sim.World, tp.Device().Id()) + if !ok { + panic(sys_error.New(fmt.Sprintf("道岔不存在: World id=%d,道岔id=%s", sim.World.Id(), tp.Device().Id()))) + } + if !entry.HasComponent(component.TurnoutPositionType) { + panic(sys_error.New(fmt.Sprintf("道岔没有TurnoutPosition组件: World id=%d,道岔id=%s", sim.World.Id(), tp.Device().Id()))) + } + // 获取定反数据 + pos := component.TurnoutPositionType.Get(entry) + switch tp.Port() { + case proto2.Port_A: + if pos.Dw { + return proto2.Port_B + } else { + return proto2.Port_C + } + case proto2.Port_B: + if pos.Dw { + return proto2.Port_A + } else { + return proto2.Port_None + } + case proto2.Port_C: + if pos.Dw { + return proto2.Port_None + } else { + return proto2.Port_A + } + } + panic(sys_error.New(fmt.Sprintf("非法端口:端口=%s", tp.Port().String()))) +} + +// 计算link offset 在道岔上的位置 +// 入参：仿真Repository、link、是否从A端开始、link偏移量、link运行方向 +// 输出：设备Id、设备所在端口、设备偏移量、公里标信息（地图主坐标系） +func calcTurnoutOffset(repo *repository.Repository, link *repository.Link, isA bool, offset int64, up bool) ( + deviceId, port string, deviceOffset int64, km *proto2.Kilometer) { tp := link.ARelation() if !isA { tp = link.BRelation() } + // 设备ID deviceId = tp.Turnout().Id() - tpOffset := tp.Turnout().FindLinkPositionByPort(tp.Port()).Offset() - crossKmInfo, portKmInfo := tp.Turnout().GetTurnoutKm(proto2.Port_None), tp.Turnout().GetTurnoutKm(tp.Port()) - portKmInfo, err := repo.ConvertKilometer(portKmInfo, crossKmInfo.CoordinateSystem) - if err != nil { - panic(err) - } - crossKm, portKm := crossKmInfo.Value, portKmInfo.Value + // 端口信息 + port = tp.Port().String() + // 设备上的偏移量 if isA { - deviceOffset = tpOffset - (tpOffset - offset) - pointTo = !up + deviceOffset = offset } else { deviceOffset = link.Length() - offset - pointTo = up } - // 查询公里标大于端口公里标 - if crossKm > portKm { - km = crossKm - deviceOffset + // 公里标计算 + crossKmInfo := convertRepoBaseKm(repo, tp.Turnout().GetTurnoutKm(proto2.Port_None)) + portKmInfo := convertRepoBaseKm(repo, tp.Turnout().GetTurnoutKm(tp.Port())) + km = &proto2.Kilometer{CoordinateSystem: crossKmInfo.CoordinateSystem} + if crossKmInfo.Value > portKmInfo.Value { + km.Value = crossKmInfo.Value - deviceOffset } else { - km = crossKm + deviceOffset - } - if up && isA { // link offset 趋大，A端岔心 ---> 边界 - runDirection = crossKm < portKm - } else if up && !isA { // link offset 趋大，B端边界 ---> 岔心 - runDirection = crossKm > portKm - } else if !up && isA { // link offset 趋小，A端边界 ---> 岔心 - runDirection = crossKm > portKm - } else { // link offset 趋小，B端岔心 ---> 边界 - runDirection = crossKm < portKm - } - switch tp.Port() { - case proto2.Port_A: - port = "A" - case proto2.Port_B: - port = "B" - case proto2.Port_C: - port = "C" + km.Value = crossKmInfo.Value + deviceOffset } return } -// 处理在道岔上link映射 -func ecsLinkMapToSection(repo *repository.Repository, offset int64, up bool, link *repository.Link) ( - deviceId, port string, deviceOffset int64, runDirection, pointTo bool, km int64) { +// 计算link offset 在区段上的位置 +// 入参：仿真Repository、link、link偏移量、link运行方向 +// 输出：设备Id、设备所在端口、设备偏移量、公里标信息（地图主坐标系） +func calcSectionOffset(repo *repository.Repository, link *repository.Link, offset int64, up bool) ( + deviceId, port string, deviceOffset int64, km *proto2.Kilometer) { var section *repository.PhysicalSection for _, s := range link.PhysicalSections() { ao, bo := s.ALinkPosition().Offset(), s.BLinkPosition().Offset() @@ -311,72 +287,258 @@ func ecsLinkMapToSection(repo *repository.Repository, offset int64, up bool, lin } } if section == nil { - panic(dto.ErrorDto{Code: dto.DataNotExist, Message: fmt.Sprintf("未找到link设备偏移【%d】", offset)}) + panic(sys_error.New(fmt.Sprintf("未找到link设备偏移【%d】", offset))) } // 元素ID deviceId = section.Id() - // link偏移变大方向 - ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() - ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer()) - akv, bkv := ak.Value, bk.Value - if up { - if ao < bo { - runDirection = akv < bkv - } else { - runDirection = akv > bkv - } - } else { - if ao > bo { - runDirection = akv < bkv - } else { - runDirection = akv > bkv - } - } - pointTo = runDirection == (akv < bkv) // a点偏移 大于 b点偏移 + ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() if ao > bo { deviceOffset = ao - offset } else { deviceOffset = offset - ao } // a点公里标 大于 b点公里标 - if akv > bkv { - km = akv - deviceOffset + ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer()) + km = &proto2.Kilometer{CoordinateSystem: ak.CoordinateSystem} + if ak.Value > bk.Value { + km.Value = ak.Value - deviceOffset } else { - km = akv + deviceOffset + km.Value = ak.Value + deviceOffset } return } -// 查找列车的公里标 -func concertTrainKilometer(kilometer, offset int64, tendTo bool) int64 { - if tendTo { - return kilometer - offset +// 根据设备查找link +// 入参：设备ID、端口、设备偏移 +// 输出：linkId、link偏移 +func QueryLinkAndOffsetByDevice(repo *repository.Repository, uid, port string, offset int64) (linkId string, linkOffset int64) { + if port == "" { + linkId, linkOffset = sectionToLink(repo, uid, offset) } else { - return kilometer + offset + linkId, linkOffset = turnoutToLink(repo, uid, port, offset) + } + return +} + +// 根据区段设备查找link +// 入参：仿真Repository、设备ID、设备偏移 +// 输出：linkId、link偏移、公里标 +func sectionToLink(repo *repository.Repository, uid string, offset int64) (string, int64) { + section := repo.FindPhysicalSection(uid) + if section == nil { + panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid))) + } + ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() + if ao < bo { + return section.ALinkPosition().Link().Id(), ao + offset + } else { + return section.ALinkPosition().Link().Id(), ao - offset } } -// 转换成统一坐标公里标 -func convertRepoBaseKm(r *repository.Repository, km *proto2.Kilometer) *proto2.Kilometer { - k, err := r.ConvertKilometer(km, r.GetCoordinateInfo().Coordinate) - if err != nil { - panic(err) +// 根据道岔设备查找link +// 入参：仿真Repository、道岔设备ID、道岔端口、设备偏移 +// 输出：linkId、link偏移 +func turnoutToLink(repo *repository.Repository, uid, port string, offset int64) (string, int64) { + turnout := repo.FindTurnout(uid) + if turnout == nil { + panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid))) + } + var portPosition *repository.LinkPosition + switch port { + case "A": + portPosition = turnout.FindLinkPositionByPort(proto2.Port_A) + case "B": + portPosition = turnout.FindLinkPositionByPort(proto2.Port_B) + case "C": + portPosition = turnout.FindLinkPositionByPort(proto2.Port_C) + default: + panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port))) + } + // 关联link + link := portPosition.Link() + isStart := link.ARelation().Device().Id() == uid + if isStart { + return link.Id(), offset + } else { + return link.Id(), link.Length() - offset } - return k } -// 检查列车是否可以放到设备上 -func checkDeviceContainTrain(offset, trainLen, deviceLen int64, deviceName string) int64 { - l := int64(math.Abs(float64(deviceLen))) - if offset > l { - panic(sys_error.New(fmt.Sprintf("偏移【%d】超出【%s】范围【%d】", offset, deviceName, l))) +// 根据设备信息、上下行，获取link运行方向，ab指向 +// 入参：仿真Repository、uid、端口、上下行 +// 输出：link运行方向、设备上的指向 +func QueryUpAndABByDevice(repo *repository.Repository, uid, port string, runDirection bool) (up, ab bool) { + if port == "" { + up, ab = runDirectionSectionToUpAndAB(repo, uid, runDirection) + } else { + up, ab = runDirectionTurnoutToUpAndAB(repo, uid, port, runDirection) } - if trainLen > offset { // 如果列车长度超过设置offset - offset = trainLen - } - if offset > l { - panic(sys_error.New(fmt.Sprintf("列车长度【%d】超出【%s】范围【%d】", trainLen, deviceName, l))) - } - return offset + return +} + +// 根据区段及上下行，获取link运行方向，ab指向 +// 入参：仿真Repository、区段UID、上下行 +// 输出：link运行方向、设备上的指向 +func runDirectionSectionToUpAndAB(repo *repository.Repository, id string, runDirection bool) (up, ab bool) { + section := repo.FindPhysicalSection(id) + if section == nil { + panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", id))) + } + ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() + // 是否从A到B，统一坐标 + ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer()) + ab = (runDirection == (ak.Value <= bk.Value)) + up = (ab == (ao < bo)) + return +} + +// 根据区段及上下行，获取link运行方向，ab指向 +// 入参：仿真Repository、道岔UID、道岔端口、上下行 +// 输出：link运行方向，设备上的指向 +func runDirectionTurnoutToUpAndAB(repo *repository.Repository, id, port string, runDirection bool) (up, ab bool) { + turnout := repo.FindTurnout(id) + if turnout == nil { + panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", id))) + } + var portKm *proto2.Kilometer + var portPosition *repository.LinkPosition + switch port { + case "A": + portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_A)) + portPosition = turnout.FindLinkPositionByPort(proto2.Port_A) + case "B": + portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_B)) + portPosition = turnout.FindLinkPositionByPort(proto2.Port_B) + case "C": + portKm = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_C)) + portPosition = turnout.FindLinkPositionByPort(proto2.Port_C) + default: + panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port))) + } + // 岔心公里标 + crossKm := convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None)) + // 是否是link的起始断点 + isStart := (portPosition.Link().ARelation() != nil && portPosition.Link().ARelation().Device().Id() == id) + up = runDirection + if (portKm.Value > crossKm.Value) != isStart { + up = !runDirection + } + ab = (portKm.Value > crossKm.Value) == runDirection + return +} + +// 根据设备以及link运行方向，获取上下行、设备上的运行指向 +// 入参：仿真Repository、道岔UID、道岔端口、上下行 +// 输出：link运行方向，设备上的指向 +func QueryDirectionAndABByDevice(repo *repository.Repository, uid, port string, up bool) (runDirection, ab bool) { + if port == "" { + runDirection, ab = upSectionToDirectionAndAB(repo, uid, up) + } else { + runDirection, ab = upTurnoutToDirectionAndAB(repo, uid, port, up) + } + return +} + +// 根据区段以及link运行方向，获取上下行、设备上的运行指向 +// 入参：仿真Repository、道岔UID、道岔端口、上下行 +// 输出：link运行方向，设备上的指向 +func upSectionToDirectionAndAB(repo *repository.Repository, uid string, up bool) (runDirection, ab bool) { + section := repo.FindPhysicalSection(uid) + if section == nil { + panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid))) + } + // a点偏移、b点偏移 + ao, bo := section.ALinkPosition().Offset(), section.BLinkPosition().Offset() + // a点公里标、b点公里标 + ak, bk := convertRepoBaseKm(repo, section.AKilometer()), convertRepoBaseKm(repo, section.BKilometer()) + if up { + runDirection = ((ao < bo) == (ak.Value < bk.Value)) + } else { + runDirection = ((ao > bo) == (ak.Value < bk.Value)) + } + ab = (runDirection == (ak.Value < bk.Value)) + return +} + +// 根据道岔以及link运行方向，获取上下行、设备上的运行指向 +// 入参：仿真Repository、道岔UID、道岔端口、上下行 +// 输出：link运行方向，设备上的指向 +func upTurnoutToDirectionAndAB(repo *repository.Repository, uid, port string, up bool) (runDirection, ab bool) { + turnout := repo.FindTurnout(uid) + if turnout == nil { + panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid))) + } + var port_enum proto2.Port + switch port { + case "A": + port_enum = proto2.Port_A + case "B": + port_enum = proto2.Port_B + case "C": + port_enum = proto2.Port_C + default: + panic(sys_error.New(fmt.Sprintf("无效端口【%s】偏移量", port))) + } + tpo := turnout.FindLinkPositionByPort(port_enum) + isA := tpo.Link().ARelation() != nil && tpo.Link().ARelation().Device().Id() == uid + crossKmInfo := convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None)) + portKmInfo := convertRepoBaseKm(repo, turnout.GetTurnoutKm(port_enum)) + if isA { + ab = !up + runDirection = (up == (crossKmInfo.Value < portKmInfo.Value)) + } else { + ab = up + runDirection = (up == (crossKmInfo.Value > portKmInfo.Value)) + } + return +} + +// 根据设备获取列车公里标 +// 入参：仿真Repository、UID、端口、上下行、link偏移 +// 输出：公里标信息 +func CalcTrainKilometer(repo *repository.Repository, uid, port string, runDirection bool, offset int64) ( + km *proto2.Kilometer) { + if port == "" { + km = sectionOffsetToKilometer(repo, uid, runDirection, offset) + } else { + km = turnoutOffsetToKilometer(repo, uid, runDirection, offset) + } + return +} + +// 获取区段的上公里标 +// 入参：仿真Repository、UID、上下行、link偏移 +// 输出：公里标信息 +func sectionOffsetToKilometer(repo *repository.Repository, uid string, runDirection bool, offset int64) (km *proto2.Kilometer) { + section := repo.FindPhysicalSection(uid) + if section == nil { + panic(sys_error.New(fmt.Sprintf("地图不存在uid:%s缓存", uid))) + } + km = convertRepoBaseKm(repo, section.AKilometer()) + if runDirection { + km.Value = km.Value + offset + } else { + km.Value = km.Value - offset + } + return +} + +// 获取道岔的公里标 +// 入参：仿真Repository、UID、上下行、link偏移 +// 输出：公里标信息 +func turnoutOffsetToKilometer(repo *repository.Repository, uid string, runDirection bool, offset int64) (km *proto2.Kilometer) { + turnout := repo.FindTurnout(uid) + if turnout == nil { + panic(sys_error.New(fmt.Sprintf("不存在道岔【uid:%s】", uid))) + } + // 岔心公里标 + km = convertRepoBaseKm(repo, turnout.GetTurnoutKm(proto2.Port_None)) + if runDirection { + km.Value = km.Value + offset + } else { + km.Value = km.Value - offset + } + return } diff --git a/ts/simulation/wayside/memory/wayside_memory_train.go b/ts/simulation/wayside/memory/wayside_memory_train.go index 41da36d..1dcc554 100644 --- a/ts/simulation/wayside/memory/wayside_memory_train.go +++ b/ts/simulation/wayside/memory/wayside_memory_train.go @@ -11,6 +11,7 @@ import ( "joylink.club/bj-rtsts-server/third_party/message" "google.golang.org/protobuf/proto" + "joylink.club/bj-rtsts-server/ts/protos/graphicData" "joylink.club/bj-rtsts-server/ts/protos/state" ) @@ -25,27 +26,53 @@ func AddTrainState(vs *VerifySimulation, status *state.TrainState, mapId int32) status.Show = true //向动力学发送初始化请求 trainIndex, _ := strconv.ParseUint(status.Id, 10, 16) + slog.Debug("添加列车", "trainIndex", trainIndex, "HeadDeviceId", status.HeadDeviceId, "HeadOffset", status.HeadOffset) // 映射link、偏移量、运行方向 - linkId, loffset, up, pointTo, kilometer := QueryEcsLinkByDeviceInfo(vs.Repo, mapId, status) + var uid string + if status.DevicePort == "" { + uid = QueryUidByMidAndComId(mapId, status.HeadDeviceId, &graphicData.Section{}) + } else { + uid = QueryUidByMidAndComId(mapId, status.HeadDeviceId, &graphicData.Turnout{}) + } + // 车头所在link、link上的偏移 + linkId, loffset := QueryLinkAndOffsetByDevice(vs.Repo, uid, status.DevicePort, status.HeadOffset) + // link上的运行方向、设备上的运行方向 + up, pointTo := QueryUpAndABByDevice(vs.Repo, uid, status.DevicePort, status.RunDirection) + // 车头所在公里标 + kilometer := CalcTrainKilometer(vs.Repo, uid, status.DevicePort, status.RunDirection, status.HeadOffset) + // 车尾相对车头link的偏移量 + var calctailOffset int64 + if up { + calctailOffset = loffset - status.TrainLength + } else { + calctailOffset = loffset + status.TrainLength + } + // 车尾位置 + tailLink, _, _, tailLOffset, _, _ := CalcInitializeLink(vs, linkId, calctailOffset, up) status.Up = up status.PointTo = pointTo - status.TrainKilometer = kilometer + status.TrainKilometer = kilometer.Value + status.DynamicState = &state.TrainDynamicState{ + HeadLinkId: linkId, + HeadLinkOffset: loffset, + TailLinkId: tailLink, + TailLinkOffset: tailLOffset, + RunningUp: up, + } + status.VobcState = &state.TrainVobcState{} + slog.Debug("列车初始化", "trainIndex", trainIndex, "linkId", linkId, "loffset", loffset) + linkIdInt, _ := strconv.Atoi(linkId) err := dynamics.Default().RequestAddTrain(&message.InitTrainInfo{ TrainIndex: uint16(trainIndex), - LinkIndex: uint16(linkId), + LinkIndex: uint16(linkIdInt), LinkOffset: uint32(loffset), Speed: uint16(math.Round(float64(status.Speed * 10))), Up: status.Up, TrainLength: uint16(status.TrainLength), }) - slog.Debug("添加列车", "trainIndex", trainIndex, "HeadDeviceId", status.HeadDeviceId, "HeadOffset", status.HeadOffset) - slog.Debug("列车初始化", "trainIndex", trainIndex, "linkId", linkId, "loffset", loffset) if err != nil { panic(dto.ErrorDto{Code: dto.DynamicsError, Message: err.Error()}) } - // 调用成功后初始化列车的动力学 - status.DynamicState = &state.TrainDynamicState{} - status.VobcState = &state.TrainVobcState{} // 将信息合并到当前设备状态中 allTrainMap.Store(status.Id, status) } diff --git a/ts/simulation/wayside/memory/wayside_simulation.go b/ts/simulation/wayside/memory/wayside_simulation.go index d574746..b93728b 100644 --- a/ts/simulation/wayside/memory/wayside_simulation.go +++ b/ts/simulation/wayside/memory/wayside_simulation.go @@ -282,14 +282,14 @@ func convert(info *message.DynamicsTrainInfo, sta *state.TrainState, simulation delayTime := time.Now().UnixMilli() - sta.VobcState.UpdateTime sta.ControlDelayTime = (int64(sta.VobcState.LifeSignal)-int64(info.VobcLifeSignal))*20 + delayTime slog.Debug("收到动力学原始消息", "Number", info.Number, "Link", info.Link, "LinkOffset", info.LinkOffset) - id, port, offset, runDirection, pointTo, kilometer := QueryDeviceByCalcLink(simulation.Repo, strconv.Itoa(int(info.Link)), int64(info.LinkOffset), info.Up) + _, id, port, _, offset, kilometer := CalcInitializeLink(simulation, strconv.Itoa(int(info.Link)), int64(info.LinkOffset), info.Up) + runDirection, pointTo := QueryDirectionAndABByDevice(simulation.Repo, id, port, info.Up) slog.Debug("处理动力学转换后的消息", "number", info.Number, "车头位置", id, "偏移", offset, "是否上行", runDirection, "是否ab", pointTo) - sta.HeadDeviceUId = id sta.HeadDeviceId = simulation.GetComIdByUid(id) sta.DevicePort = port sta.HeadOffset = offset sta.PointTo = pointTo - sta.TrainKilometer = kilometer + sta.TrainKilometer = kilometer.Value sta.RunDirection = runDirection //判定车头方向 sta.HeadDirection = runDirection