419 lines
14 KiB
Go
419 lines
14 KiB
Go
package simulation
|
|
|
|
import (
|
|
"encoding/binary"
|
|
"encoding/hex"
|
|
"joylink.club/rtsssimulation/repository"
|
|
"joylink.club/rtsssimulation/repository/model/proto"
|
|
"math"
|
|
"time"
|
|
|
|
"fmt"
|
|
"net/http"
|
|
"strconv"
|
|
"sync"
|
|
|
|
"go.uber.org/zap"
|
|
"joylink.club/bj-rtsts-server/ats/verify/protos/state"
|
|
"joylink.club/bj-rtsts-server/ats/verify/simulation/wayside/memory"
|
|
"joylink.club/bj-rtsts-server/config"
|
|
"joylink.club/bj-rtsts-server/dynamics"
|
|
"joylink.club/bj-rtsts-server/vobc"
|
|
|
|
"joylink.club/bj-rtsts-server/dto"
|
|
)
|
|
|
|
func init() {
|
|
// vobc 发来的列车信息
|
|
vobc.RegisterTrainInfoHandler(func(info []byte) {
|
|
zap.S().Info("接收到vobc发送的列车消息", hex.EncodeToString(info))
|
|
memory.UdpUpdateTime.VobcTime = time.Now().UnixMilli()
|
|
for _, simulation := range GetSimulationArr() {
|
|
simulation.Memory.Status.TrainStateMap.Range(func(_, value any) bool {
|
|
train := value.(*state.TrainState)
|
|
if !train.Show { // 下线列车
|
|
return false
|
|
}
|
|
// 拼接列车ID
|
|
trainId, err := strconv.Atoi(train.Id)
|
|
if err != nil {
|
|
panic(dto.ErrorDto{Code: dto.ArgumentParseError, Message: err.Error()})
|
|
}
|
|
trainInfo := decoderVobcTrainState(info)
|
|
zap.S().Info("解析后vobc列车消息", trainInfo)
|
|
d := append(info, uint8(trainId))
|
|
// 发送给动力学
|
|
dynamics.SendDynamicsTrainMsg(d)
|
|
zap.S().Info("发送后：", hex.EncodeToString(d))
|
|
// 存放至列车中
|
|
train.VobcState = trainInfo
|
|
return true
|
|
})
|
|
}
|
|
})
|
|
dynamics.RegisterTrainInfoHandler(func(info *dynamics.TrainInfo) {
|
|
zap.S().Info("发送到vobc的列车编号为", info.Number)
|
|
memory.UdpUpdateTime.DynamicsTime = time.Now().UnixMilli()
|
|
for _, simulation := range GetSimulationArr() {
|
|
sta, ok := simulation.Memory.Status.TrainStateMap.Load(strconv.Itoa(int(info.Number)))
|
|
if !ok {
|
|
continue
|
|
}
|
|
trainState := sta.(*state.TrainState)
|
|
// 给半实物仿真发送速度
|
|
zap.S().Infof("发送到vobc发送的速度%d, %d \n", info.Speed, math.Abs(float64(info.Speed*36)))
|
|
vobc.SendTrainSpeedTask(math.Abs(float64(info.Speed * 36)))
|
|
// 更新列车状态
|
|
memory.UpdateTrainState(simulation, convert(info, trainState, simulation))
|
|
}
|
|
})
|
|
}
|
|
|
|
// 仿真存储集合
|
|
var simulationMap sync.Map
|
|
|
|
// 创建前检查
|
|
func IsExistSimulation() bool {
|
|
i := 0
|
|
simulationMap.Range(func(_, _ any) bool {
|
|
i++
|
|
return true
|
|
})
|
|
return i > 0
|
|
}
|
|
|
|
// 创建仿真对象
|
|
func CreateSimulation(projectId int32, mapIds []int32) string {
|
|
simulationId := createSimulationId(projectId)
|
|
_, e := simulationMap.Load(simulationId)
|
|
if !e && IsExistSimulation() {
|
|
panic(dto.ErrorDto{Code: dto.DataAlreadyExist, Message: "已有仿真在运行"})
|
|
}
|
|
if !e {
|
|
verifySimulation, err := memory.CreateSimulation(projectId, mapIds)
|
|
if err != nil {
|
|
panic(fmt.Sprintf("创建仿真失败:%s", err.Error()))
|
|
}
|
|
verifySimulation.SimulationId = simulationId
|
|
//通知动力学
|
|
lineBaseInfo := buildLineBaseInfo(verifySimulation.Repo)
|
|
httpCode, _, err := dynamics.SendSimulationStartReq(lineBaseInfo)
|
|
if httpCode != http.StatusOK || err != nil {
|
|
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
|
|
}
|
|
|
|
simulationMap.Store(simulationId, verifySimulation)
|
|
dynamicsRun(verifySimulation)
|
|
}
|
|
return simulationId
|
|
|
|
//simulationId := createSimulationId(projectId)
|
|
//_, e := simulationMap.Load(simulationId)
|
|
//if !e && IsExistSimulation() {
|
|
// panic(dto.ErrorDto{Code: dto.DataAlreadyExist, Message: "已有仿真在运行"})
|
|
//}
|
|
//if !e {
|
|
// verifySimulation := memory.CreateSimulation(projectId, mapIds)
|
|
// verifySimulation.SimulationId = simulationId
|
|
// lineDataInfo := &dynamics.LineBaseInfo{}
|
|
// for _, mapId := range mapIds {
|
|
// b := buildLineBaseInfo(memory.QueryMapVerifyStructure(mapId))
|
|
// lineDataInfo.CurveList = append(lineDataInfo.CurveList, b.CurveList...)
|
|
// lineDataInfo.LinkList = append(lineDataInfo.LinkList, b.LinkList...)
|
|
// lineDataInfo.SlopeList = append(lineDataInfo.SlopeList, b.SlopeList...)
|
|
// }
|
|
// //通知动力学
|
|
// httpCode, _, err := dynamics.SendSimulationStartReq(lineDataInfo)
|
|
// if httpCode != http.StatusOK || err != nil {
|
|
// panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
|
|
// }
|
|
// simulationMap.Store(simulationId, verifySimulation)
|
|
// dynamicsRun(verifySimulation)
|
|
//}
|
|
//return simulationId
|
|
}
|
|
|
|
// 删除仿真对象
|
|
func DestroySimulation(simulationId string) {
|
|
simulationMap.Delete(simulationId)
|
|
//移除道岔状态发送
|
|
dynamics.Stop()
|
|
//通知动力学
|
|
httpCode, _, err := dynamics.SendSimulationEndReq()
|
|
if httpCode != http.StatusOK || err != nil {
|
|
panic(dto.ErrorDto{Code: dto.DynamicsError, Message: fmt.Sprintf("动力学接口调用失败：[%d][%s]", httpCode, err)})
|
|
}
|
|
}
|
|
|
|
func createSimulationId(projectId int32) string {
|
|
// 当前服务器IP + 端口 + 项目
|
|
return config.SimulationId_prefix + "_" + strconv.Itoa(config.Config.Server.Port) + "_" + strconv.Itoa(int(projectId))
|
|
}
|
|
|
|
// 获取仿真列表
|
|
func ListAllSimulations() []*dto.SimulationInfoRspDto {
|
|
var simArr []*dto.SimulationInfoRspDto
|
|
simulationMap.Range(func(_, v any) bool {
|
|
s := v.(*memory.VerifySimulation)
|
|
simArr = append(simArr, &dto.SimulationInfoRspDto{
|
|
SimulationId: s.SimulationId,
|
|
MapId: s.MapIds[0],
|
|
MapIds: s.MapIds,
|
|
ProjectId: s.ProjectId,
|
|
})
|
|
return true
|
|
})
|
|
return simArr
|
|
}
|
|
|
|
// 根据仿真id查找仿真实例
|
|
func FindSimulation(simulationId string) *memory.VerifySimulation {
|
|
m, e := simulationMap.Load(simulationId)
|
|
if e {
|
|
return m.(*memory.VerifySimulation)
|
|
}
|
|
return nil
|
|
}
|
|
|
|
// 获取普通仿真数组
|
|
func GetSimulationArr() []*memory.VerifySimulation {
|
|
var result []*memory.VerifySimulation
|
|
simulationMap.Range(func(_, v any) bool {
|
|
result = append(result, v.(*memory.VerifySimulation))
|
|
return true
|
|
})
|
|
return result
|
|
}
|
|
|
|
func convert(info *dynamics.TrainInfo, sta *state.TrainState, simulation *memory.VerifySimulation) *state.TrainState {
|
|
//var vs *memory.VerifyStructure
|
|
//linkId := int32(info.Link)
|
|
//for _, mapId := range simulation.MapIds {
|
|
// vm := memory.QueryMapVerifyStructure(mapId)
|
|
// if vm.LinkModelMap[linkId] == nil {
|
|
// continue
|
|
// }
|
|
// vs = vm
|
|
// break
|
|
//}
|
|
//zap.S().Debugf("原始消息：[%d-%d-%d]", info.Number, info.Link, info.LinkOffset)
|
|
//id, port, offset, runDirection, pointTo, kilometer := memory.QueryDeviceByCalcLink(vs, linkId, int64(info.LinkOffset), info.Up, sta.RunDirection)
|
|
//zap.S().Debugf("转换后的消息：[%d-车头:%s-偏移:%d-上行:%v-是否ab:%v]", info.Number, id, offset, runDirection, pointTo)
|
|
//sta.HeadDeviceId = id
|
|
//sta.DevicePort = port
|
|
//sta.HeadOffset = offset
|
|
//sta.PointTo = pointTo
|
|
//sta.TrainKilometer = kilometer
|
|
//sta.RunDirection = runDirection
|
|
////判定车头方向
|
|
//sta.HeadDirection = runDirection
|
|
//if sta.VobcState != nil {
|
|
// if sta.VobcState.DirectionForward {
|
|
// sta.HeadDirection = runDirection
|
|
// } else if sta.VobcState.DirectionBackward {
|
|
// sta.HeadDirection = !runDirection
|
|
// }
|
|
//}
|
|
//if info.Speed < 0 {
|
|
// sta.RunDirection = !sta.RunDirection
|
|
//}
|
|
//// 赋值动力学信息
|
|
//sta.DynamicState.Heartbeat = int32(info.LifeSignal)
|
|
//sta.DynamicState.HeadLinkId = strconv.Itoa(int(info.Link))
|
|
//sta.DynamicState.HeadLinkOffset = int64(info.LinkOffset)
|
|
//sta.DynamicState.Slope = int32(info.Slope)
|
|
//sta.DynamicState.Upslope = info.UpSlope
|
|
//sta.DynamicState.RunningUp = info.Up
|
|
//sta.DynamicState.RunningResistanceSum = float32(info.TotalResistance) / 1000
|
|
//sta.DynamicState.AirResistance = float32(info.AirResistance) / 1000
|
|
//sta.DynamicState.RampResistance = float32(info.SlopeResistance) / 1000
|
|
//sta.DynamicState.CurveResistance = float32(info.CurveResistance) / 1000
|
|
//sta.DynamicState.Speed = speedParse(info.Speed)
|
|
//sta.DynamicState.HeadSensorSpeed1 = speedParse(info.HeadSpeed1)
|
|
//sta.DynamicState.HeadSensorSpeed2 = speedParse(info.HeadSpeed2)
|
|
//sta.DynamicState.TailSensorSpeed1 = speedParse(info.TailSpeed1)
|
|
//sta.DynamicState.TailSensorSpeed2 = speedParse(info.TailSpeed2)
|
|
//sta.DynamicState.HeadRadarSpeed = speedParse(info.HeadRadarSpeed)
|
|
//sta.DynamicState.TailRadarSpeed = speedParse(info.TailRadarSpeed)
|
|
return sta
|
|
}
|
|
|
|
func dynamicsRun(verifySimulation *memory.VerifySimulation) {
|
|
_ = dynamics.Run(func() []*dynamics.TurnoutInfo {
|
|
stateSlice := memory.GetAllTurnoutState(verifySimulation)
|
|
var turnoutInfoSlice []*dynamics.TurnoutInfo
|
|
for _, sta := range stateSlice {
|
|
code64, err := strconv.ParseUint(sta.Id, 10, 16)
|
|
if err != nil {
|
|
zap.S().Error("id转uint16报错", err)
|
|
}
|
|
info := dynamics.TurnoutInfo{
|
|
Code: uint16(code64),
|
|
NPosition: sta.Normal,
|
|
RPosition: sta.Reverse,
|
|
}
|
|
turnoutInfoSlice = append(turnoutInfoSlice, &info)
|
|
}
|
|
return turnoutInfoSlice
|
|
})
|
|
}
|
|
|
|
func buildLineBaseInfo(repo *repository.Repository) *dynamics.LineBaseInfo {
|
|
info := &dynamics.LineBaseInfo{}
|
|
for _, model := range repo.LinkList() {
|
|
id, _ := strconv.Atoi(model.Id())
|
|
link := &dynamics.Link{
|
|
ID: int32(id),
|
|
Len: int32(model.Length()),
|
|
}
|
|
info.LinkList = append(info.LinkList, link)
|
|
if model.ARelation() != nil {
|
|
turnoutId, _ := strconv.Atoi(model.ARelation().Device().Id())
|
|
link.ARelTurnoutId = int32(turnoutId)
|
|
switch model.ARelation().Port() {
|
|
case proto.Port_A:
|
|
link.ARelTurnoutPoint = "A"
|
|
case proto.Port_B:
|
|
link.ARelTurnoutPoint = "B"
|
|
case proto.Port_C:
|
|
link.ARelTurnoutPoint = "C"
|
|
}
|
|
}
|
|
if model.BRelation() != nil {
|
|
turnoutId, _ := strconv.Atoi(model.BRelation().Device().Id())
|
|
link.BRelTurnoutId = int32(turnoutId)
|
|
switch model.BRelation().Port() {
|
|
case proto.Port_A:
|
|
link.BRelTurnoutPoint = "A"
|
|
case proto.Port_B:
|
|
link.BRelTurnoutPoint = "B"
|
|
case proto.Port_C:
|
|
link.BRelTurnoutPoint = "C"
|
|
}
|
|
}
|
|
}
|
|
for _, model := range repo.SlopeList() {
|
|
id, _ := strconv.Atoi(model.Id())
|
|
slope := &dynamics.Slope{
|
|
ID: int32(id),
|
|
StartLinkOffset: int32(model.StartLinkPosition().Offset()),
|
|
EndLinkOffset: int32(model.EndLinkPosition().Offset()),
|
|
DegreeTrig: model.Degree(),
|
|
}
|
|
info.SlopeList = append(info.SlopeList, slope)
|
|
startLinkId, _ := strconv.Atoi(model.StartLinkPosition().Link().Id())
|
|
slope.StartLinkId = int32(startLinkId)
|
|
endLinkId, _ := strconv.Atoi(model.EndLinkPosition().Link().Id())
|
|
slope.EndLinkId = int32(endLinkId)
|
|
}
|
|
for _, model := range repo.SectionalCurvatureList() {
|
|
id, _ := strconv.Atoi(model.Id())
|
|
curve := &dynamics.Curve{
|
|
ID: int32(id),
|
|
StartLinkOffset: int32(model.StartLinkPosition().Offset()),
|
|
EndLinkOffset: int32(model.EndLinkPosition().Offset()),
|
|
Curvature: model.Radius(),
|
|
}
|
|
info.CurveList = append(info.CurveList, curve)
|
|
startLinkId, _ := strconv.Atoi(model.StartLinkPosition().Link().Id())
|
|
curve.StartLinkId = int32(startLinkId)
|
|
endLinkId, _ := strconv.Atoi(model.EndLinkPosition().Link().Id())
|
|
curve.EndLinkId = int32(endLinkId)
|
|
}
|
|
return info
|
|
}
|
|
|
|
//func buildLineBaseInfo(vs *memory.VerifyStructure) *dynamics.LineBaseInfo {
|
|
// var links []*dynamics.Link
|
|
// var slopes []*dynamics.Slope
|
|
// var curves []*dynamics.Curve
|
|
// for _, link := range vs.LinkModelMap {
|
|
// id, _ := strconv.Atoi(link.Index)
|
|
// var aTurnoutId int
|
|
// var aPort string
|
|
// if link.ARelatedSwitchRef != nil && link.ARelatedSwitchRef.SwitchDevice != nil {
|
|
// aTurnoutId, _ = strconv.Atoi(link.ARelatedSwitchRef.SwitchDevice.GetIndex())
|
|
// aPort = link.ARelatedSwitchRef.Port.Name()
|
|
// }
|
|
// var bTurnoutId int
|
|
// var bPort string
|
|
// if link.BRelatedSwitchRef != nil && link.BRelatedSwitchRef.SwitchDevice != nil {
|
|
// bTurnoutId, _ = strconv.Atoi(link.BRelatedSwitchRef.SwitchDevice.GetIndex())
|
|
// bPort = link.BRelatedSwitchRef.Port.Name()
|
|
// }
|
|
// links = append(links, &dynamics.Link{
|
|
// ID: int32(id),
|
|
// Len: link.Length,
|
|
// ARelTurnoutId: int32(aTurnoutId),
|
|
// ARelTurnoutPoint: aPort,
|
|
// BRelTurnoutId: int32(bTurnoutId),
|
|
// BRelTurnoutPoint: bPort,
|
|
// })
|
|
// }
|
|
// 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,
|
|
// CurveList: curves,
|
|
// }
|
|
//}
|
|
|
|
// 解析VOBC列车信息
|
|
func decoderVobcTrainState(buf []byte) *state.TrainVobcState {
|
|
trainVobcInfo := &state.TrainVobcState{}
|
|
trainVobcInfo.LifeSignal = int32(binary.BigEndian.Uint16(buf[0:2]))
|
|
b2 := buf[2]
|
|
trainVobcInfo.Tc1Active = (b2 & 1) != 0
|
|
trainVobcInfo.Tc2Active = (b2 & (1 << 1)) != 0
|
|
trainVobcInfo.DirectionForward = (b2 & (1 << 2)) != 0
|
|
trainVobcInfo.DirectionBackward = (b2 & (1 << 3)) != 0
|
|
trainVobcInfo.TractionStatus = (b2 & (1 << 4)) != 0
|
|
trainVobcInfo.BrakingStatus = (b2 & (1 << 5)) != 0
|
|
trainVobcInfo.EmergencyBrakingStatus = (b2 & (1 << 6)) != 0
|
|
trainVobcInfo.TurnbackStatus = (b2 & 7) != 0
|
|
b3 := buf[3]
|
|
trainVobcInfo.JumpStatus = (b3 & 1) != 0
|
|
trainVobcInfo.Ato = (b3 & (1 << 1)) != 0
|
|
trainVobcInfo.Fam = (b3 & (1 << 2)) != 0
|
|
trainVobcInfo.Cam = (b3 & (1 << 3)) != 0
|
|
trainVobcInfo.TractionSafetyCircuit = (b3 & (1 << 4)) != 0
|
|
trainVobcInfo.ParkingBrakeStatus = (b3 & (1 << 5)) != 0
|
|
trainVobcInfo.MaintainBrakeStatus = (b3 & (1 << 6)) != 0
|
|
trainVobcInfo.TractionForce = int64(binary.BigEndian.Uint16(buf[4:6]))
|
|
trainVobcInfo.BrakeForce = int64(binary.BigEndian.Uint16(buf[6:8]))
|
|
trainVobcInfo.TrainLoad = int64(binary.BigEndian.Uint16(buf[8:10]))
|
|
b4 := buf[15]
|
|
trainVobcInfo.LeftDoorOpenCommand = (b4 & 1) != 0
|
|
trainVobcInfo.RightDoorOpenCommand = (b4 & (1 << 1)) != 0
|
|
trainVobcInfo.LeftDoorCloseCommand = (b4 & (1 << 2)) != 0
|
|
trainVobcInfo.RightDoorCloseCommand = (b4 & (1 << 3)) != 0
|
|
trainVobcInfo.AllDoorClose = (b4 & (1 << 4)) != 0
|
|
return trainVobcInfo
|
|
}
|
|
|
|
// 发送给前端的速度格式化
|
|
func speedParse(speed float32) int32 {
|
|
return int32(math.Abs(float64(speed * 3.6 * 100)))
|
|
}
|