dcl_dispatch_server/src/package/util/u_xosc.go

package util

import (
	"fmt"
	"github.com/beevik/etree"
	"strings"
)

var (
	xodr1Key   = "OpenDRIVE2_add_stop_line_marking_20231229.xodr"
	xodr1Value = "resource/OpenDRIVE2_add_stop_line_marking_20231229.xodr"
	xodr2Key   = "thq_1116.xodr"
	xodr2Value = "resource/thq_1116.xodr"
	xodr3Key   = "thq_20240706.xodr"
	xodr3Value = "resource/thq_20240706.xodr"
	xodr4Key   = "yizhuang.xodr"
	xodr4Value = "resource/yizhuang.xodr"
	xodr5Key   = "anqing.xodr"
	xodr5Value = "resource/anqing.xodr"
	osgb1Key   = "jinlong_all_addtrafficlight_20240226_5_guazai_2.opt.osgb"
	osgb1Value = "resource/jinlong_all_addtrafficlight_20240226_5_guazai_2.opt.osgb"
	osgb2Key   = "thq_1116.opt.osgb"
	osgb2Value = "resource/thq_1116.opt.osgb"
	osgb3Key   = "thq_20230710.osgb"
	osgb3Value = "resource/thq_20230710.osgb"
	osgb4Key   = "yizhuang.osgb"
	osgb4Value = "resource/yizhuang.osgb"
	osgb5Key   = "anqing.osgb"
	osgb5Value = "resource/anqing.osgb"
)

// - filepath  文件路径 "D:\\test.xosc"
func ParseXosc(xoscPath string) (string, string, string, string, string, string, string, string, string, string) {
	startPositionX := ""
	startPositionY := ""
	startPositionZ := ""
	startPositionH := ""
	endPositionX := ""
	endPositionY := ""
	endPositionZ := ""
	endPositionH := ""
	xodrPath := ""
	osgbPath := ""
	doc := etree.NewDocument()
	if err := doc.ReadFromFile(xoscPath); err != nil {
		panic(err)
	}
	storys := doc.SelectElement("OpenSCENARIO").SelectElement("Storyboard").SelectElements("Story")
	for _, story := range storys {
		for _, attr := range story.Attr {
			if attr.Key == "name" && attr.Value == "mystory_ego" {
				events := story.SelectElement("Act").SelectElement("ManeuverGroup").SelectElement("Maneuver").SelectElements("Event")
				for _, event := range events {
					for _, attr2 := range event.Attr {
						if attr2.Key == "name" && attr2.Value == "Event1" {
							vertexs := event.SelectElement("Action").SelectElement("PrivateAction").SelectElement("RoutingAction").SelectElement("FollowTrajectoryAction").SelectElement("Trajectory").SelectElement("Shape").SelectElement("Polyline").SelectElements("Vertex")
							for i, vertex := range vertexs {
								if i == 0 {
									worldPosition := vertex.SelectElement("Position").SelectElement("WorldPosition")
									for _, attr3 := range worldPosition.Attr {
										fmt.Printf("【%v】", attr3)
										if attr3.Key == "x" {
											startPositionX = attr3.Value
										}
										if attr3.Key == "y" {
											startPositionY = attr3.Value
										}
										if attr3.Key == "z" {
											startPositionZ = attr3.Value
										}
										if attr3.Key == "h" {
											startPositionH = attr3.Value
										}
									}
								}
								if i == len(vertexs)-1 {
									worldPosition := vertex.SelectElement("Position").SelectElement("WorldPosition")
									for _, attr3 := range worldPosition.Attr {
										fmt.Printf("【%v】", attr3)
										if attr3.Key == "x" {
											endPositionX = attr3.Value
										}
										if attr3.Key == "y" {
											endPositionY = attr3.Value
										}
										if attr3.Key == "z" {
											endPositionZ = attr3.Value
										}
										if attr3.Key == "h" {
											endPositionH = attr3.Value
										}
									}
								}
							}
						}
					}
				}
			}
		}
	}

	logicFile := doc.SelectElement("OpenSCENARIO").SelectElement("RoadNetwork").SelectElement("LogicFile")
	for _, attr := range logicFile.Attr {
		if attr.Key == "filepath" {
			xodrKey := attr.Value
			xodrPath = getValueByKey(xodrKey)
		}
	}
	sceneGraphFile := doc.SelectElement("OpenSCENARIO").SelectElement("RoadNetwork").SelectElement("SceneGraphFile")
	for _, attr := range sceneGraphFile.Attr {
		if attr.Key == "filepath" {
			osgbKey := attr.Value
			osgbPath = getValueByKey(osgbKey)
		}
	}

	return startPositionX, startPositionY, startPositionZ, startPositionH, endPositionX, endPositionY, endPositionZ, endPositionH, xodrPath, osgbPath
}

func getValueByKey(key string) string {
	if strings.Contains(key, xodr1Key) {
		return xodr1Value
	}
	if strings.Contains(key, xodr2Key) {
		return xodr2Value
	}
	if strings.Contains(key, xodr3Key) {
		return xodr3Value
	}
	if strings.Contains(key, xodr4Key) {
		return xodr4Value
	}
	if strings.Contains(key, xodr5Key) {
		return xodr5Value
	}
	if strings.Contains(key, osgb1Key) {
		return osgb1Value
	}
	if strings.Contains(key, osgb2Key) {
		return osgb2Value
	}
	if strings.Contains(key, osgb3Key) {
		return osgb3Value
	}
	if strings.Contains(key, osgb4Key) {
		return osgb4Value
	}
	if strings.Contains(key, osgb5Key) {
		return osgb5Value
	}
	fmt.Printf("错误的xosc【%v】", key)
	return ""
}