Java解析OpenDrive，OpenDrive格式解析

1 介绍

项目地址：https://gitee.com/AiShiYuShiJiePingXing/open-drive-convert

分支：dev

找了很多笔记和参考资料，解析OpenDrive得很多开源项目几乎都是C++或者Python做的，奈何那两块都没那么熟悉，只能通过Java实现了，参考了一个项目，实现了Java解析OpenDrive。

1.1 运行

clone项目：

git clone -b dev https://gitee.com/AiShiYuShiJiePingXing/open-drive-convert.git

找到/src/main/java/OpenDriveConvert.java

修改openDrivePath得路径，运行main方法即可完成测试。

2 Java解析OpenDrive

2.1 pom.xml

先添加几个依赖，下酒菜。

<dependencies>
        <dependency>
            <groupId>com.alibaba</groupId>
            <artifactId>fastjson</artifactId>
            <version>1.2.69</version>
        </dependency>
        <dependency>
            <groupId>commons-io</groupId>
            <artifactId>commons-io</artifactId>
            <version>2.6</version>
        </dependency>
        <dependency>
            <groupId>org.projectlombok</groupId>
            <artifactId>lombok</artifactId>
            <version>RELEASE</version>
        </dependency>
        <dependency>
            <groupId>ch.qos.logback</groupId>
            <artifactId>logback-classic</artifactId>
            <version>1.2.3</version>
        </dependency>
        <dependency>
            <groupId>cn.hutool</groupId>
            <artifactId>hutool-all</artifactId>
            <version>5.8.0</version>
        </dependency>
    </dependencies>

2.2 读取本地xodr文件

public static String readFromFile(String xodrPath) {
    log.info("OpenDRIVE地图文件路径：{}", xodrPath);
	//xodrPath：eg:D://project//test.xodr
    String xodrStr = null;
    try {
        xodrStr = FileUtils.readFileToString(new File(xodrPath), "UTF-8");
    } catch (IOException e) {
        e.printStackTrace();
    }

    return xodrStr;
}

2.3 开始解析Map

public static MapDataContainer parse(String input) {
    log.info("开始解析地图...");
    List<Road> roads = new ArrayList<>();
    List<Junction> junctions = new ArrayList<>();
    List<LaneSection> laneSections = new ArrayList<>();
    List<Lane> lanes = new ArrayList<>();
    List<Connection> connections = new ArrayList<>();
    List<LaneLink> laneLinks = new ArrayList<>();

    roadMap = new HashMap<>();
    laneSectionMap = new HashMap<>();
    laneMap = new HashMap<>();
    junctionMap = new HashMap<>();

    laneSectionId = 0;
    laneSingleId = 0;

    roadIndex = 0;
    laneSectionIndex = 0;
    laneIndex = 0;
    junctionIndex = 0;
    connectionIndex = 0;
    laneLinkIndex = 0;

    DocumentBuilderFactory factory = DocumentBuilderFactory.newInstance();
    try {
        DocumentBuilder documentBuilder = factory.newDocumentBuilder();
        Document document = documentBuilder.parse(new InputSource(new StringReader(input)));
        Element root = document.getDocumentElement();

        // 1. 解析road及子元素
        NodeList roadList = root.getElementsByTagName("road");
        parseRoad(roadList, roads, laneSections, lanes);

        // 2. 解析junction及子元素
        NodeList junctionList = root.getElementsByTagName("junction");
        parseJunction(junctionList, junctions, connections, laneLinks);

        // 3. 初始化index; 初始化connection的direction
        initIndex(roads, laneSections, lanes, junctions, connections, laneLinks);

        log.info("解析地图完成！");
        return new MapDataContainer(roads, junctions, laneSections, lanes, connections, laneLinks);

    } catch (ParserConfigurationException | SAXException | IOException e) {
        e.printStackTrace();
        log.error("解析OpenDRIVE文件时发生错误！");
        return null;
    }

}

2.4 解析Road

private static void parseRoad(NodeList roadList, List<Road> roads, List<LaneSection> laneSections, List<Lane> lanes) {
    try {
        for (int i = 0; i < roadList.getLength(); i++) {
            Element roadElement = (Element) roadList.item(i);
            Road road = new Road();

            //    int elementType;
            road.setElementType(ElementType.ROAD.getValue());
            //    int roadId;
            int roadId = Integer.parseInt(roadElement.getAttribute("id"));
            road.setRoadId(roadId);

            road.setIndex(roadIndex); // 初始化index
            roadMap.put(roadId, roadIndex++);

            //    int junctionId;
            int junctionId = Integer.parseInt(roadElement.getAttribute("junction"));
            road.setJunctionId(junctionId);
            //    int length;
            double length = Double.parseDouble(roadElement.getAttribute("length"));
            road.setLength(length);
            //    int predecessorElementType;
            //    int successorElementType;
            int predecessorId = -1;
            int successorId = -1;
            ElementType predecessorElementType = ElementType.NONE;
            ElementType successorElementType = ElementType.NONE;

            NodeList linkList = roadElement.getElementsByTagName("link");
            Element myLinkList = (Element) linkList.item(0);
            NodeList predecessorList = null, successorList = null;
            if (myLinkList != null) {
                predecessorList = myLinkList.getElementsByTagName("predecessor");
                successorList = myLinkList.getElementsByTagName("successor");
            }

            if (predecessorList != null) {
                Element predecessor = (Element) predecessorList.item(0);
                if (predecessor != null) {
                    String elementId = predecessor.getAttribute("elementId");
                    String elementType = predecessor.getAttribute("elementType");
                    if (elementId != null && elementType != null && elementId.length() != 0 && elementType.length() != 0) {
                        predecessorId = Integer.parseInt(elementId);
                        predecessorElementType = elementType.equalsIgnoreCase("junction")
                            ? ElementType.JUNCTION : ElementType.ROAD;
                        road.setPredecessorElementType(predecessorElementType.getValue());
                    }
                }
            }
            road.setPredecessorElementType(predecessorElementType.getValue());
            road.setPredecessorId(predecessorId);

            if (successorList != null) {
                Element successor = (Element) successorList.item(0);
                if (successor != null) {
                    String elementId = successor.getAttribute("elementId");
                    String elementType = successor.getAttribute("elementType");
                    if (elementId != null && elementType != null && elementId.length() != 0 && elementType.length() != 0) {
                        successorId = Integer.parseInt(elementId);
                        successorElementType = elementType.equalsIgnoreCase("junction")
                            ? ElementType.JUNCTION : ElementType.ROAD;
                    }
                }
            }
            road.setSuccessorElementType(successorElementType.getValue());
            road.setSuccessorId(successorId);

            //    int maxSpeed;
            NodeList typeNodeList = roadElement.getElementsByTagName("type");
            if (typeNodeList != null) {
                Element typeElement = (Element) typeNodeList.item(0);
                if (typeElement != null) {
                    NodeList speedNodeList = typeElement.getElementsByTagName("speed");
                    if (speedNodeList != null) {
                        Element speedElement = (Element) speedNodeList.item(0);
                        double maxSpeed = Double.parseDouble(speedElement.getAttribute("max"));
                        road.setMaxSpeed(maxSpeed);
                    }
                }
            } else {
                road.setMaxSpeed(UppaalUtil.INT16_MAX * 1.0 / UppaalUtil.K);
            }

            // laneSections
            NodeList lanesNodeList = roadElement.getElementsByTagName("lanes");
            Element lanesElement = (Element) lanesNodeList.item(0);
            NodeList laneSectionList = lanesElement.getElementsByTagName("laneSection");
            parseLaneSection(laneSectionList, road, laneSections, lanes);

            roads.add(road);
        }
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.5 解析车道属性

private static void parseLaneSection(NodeList laneSectionList, Road road, List<LaneSection> laneSections, List<Lane> lanes) {
    try {
        List<LaneSection> roadLaneSections = new ArrayList<>();
        List<Integer> laneSectionsIndex = new ArrayList<>();
        for (int i = 0; i < laneSectionList.getLength(); i++) {
            Element laneSectionElement = (Element) laneSectionList.item(i);
            LaneSection laneSection = new LaneSection();
            //private int elementType;
            laneSection.setElementType(ElementType.LANE_SECTION.getValue());
            //private int roadIndex;
            //private int roadId;
            laneSection.setRoadId(road.getRoadId());
            laneSection.setRoadIndex(road.getIndex());

            //private int laneSectionId;
            laneSection.setLaneSectionId(laneSectionId);
            laneSection.setIndex(laneSectionIndex);
            laneSectionsIndex.add(laneSectionIndex);
            laneSectionMap.put(laneSectionId++, laneSectionIndex++);

            //private double startPosition;
            double s = Double.parseDouble(laneSectionElement.getAttribute("s"));
            laneSection.setStartPosition(s);

            //private double length;
            double length = 0; // 本车道段起始位置减去上一个的起始位置
            laneSection.setLength(length);

            // lanes
            NodeList laneList = laneSectionElement.getElementsByTagName("lane");
            parseLane(laneList, laneSection, lanes);

            roadLaneSections.add(laneSection);
            laneSections.add(laneSection);
        }
        road.setLaneSectionsIndex(laneSectionsIndex);
        road.setLaneSections(roadLaneSections);
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.6 解析车道

private static void parseLane(NodeList laneList, LaneSection laneSection, List<Lane> lanes) {
    try {
        List<Lane> laneSectionLanes = new ArrayList<>();
        List<Integer> lanesIndex = new ArrayList<>();
        for (int i = 0; i < laneList.getLength(); i++) {
            Element laneElement = (Element) laneList.item(i);
            Lane lane = new Lane();

            //                private int elementType;
            lane.setElementType(ElementType.LANE.getValue());
            //                private int roadId;
            lane.setRoadId(laneSection.getRoadId());
            //                private int roadIndex;
            lane.setRoadIndex(laneSection.getRoadIndex());
            //                private int laneSectionIndex;
            lane.setLaneSectionIndex(laneSection.getIndex());
            //                private int laneSectionId;
            lane.setLaneSectionId(laneSection.getLaneSectionId());
            //                private int laneId;
            lane.setLaneId(Integer.parseInt(laneElement.getAttribute("id")));

            lane.setSingleId(laneSingleId); //上面的setLaneId中是相对位置，这里是标识符
            lane.setIndex(laneIndex);
            lanesIndex.add(laneIndex);
            laneMap.put(laneSingleId++, laneIndex++);

            //                private int type;
            String type = laneElement.getAttribute("type");
            if (type.equals("driving")) {
                lane.setType(1);
            } else {
                lane.setType(0);
            }

            //                private int predecessorIndex;
            //                private int predecessorLaneId; // 与laneId同类，表示相对位置
            //                private int predecessorId; // 标识符
            NodeList predecessorList = laneElement.getElementsByTagName("predecessor");
            String predecessorLaneId;
            if (predecessorList != null) {
                Element predecessorElement = (Element) predecessorList.item(0);
                if (predecessorElement != null) {
                    predecessorLaneId = predecessorElement.getAttribute("id");
                    lane.setPredecessorLaneId(Integer.parseInt(predecessorLaneId));
                }
            }

            //                private int successorIndex; 
            //                private int successorLaneId;
            //                private int successorId;
            NodeList successorList = laneElement.getElementsByTagName("successor");
            String successorLaneId;
            if (successorList != null) {
                Element successorElement = (Element) successorList.item(0);
                if (successorElement != null) {
                    successorLaneId = successorElement.getAttribute("id");
                    lane.setSuccessorLaneId(Integer.parseInt(successorLaneId));
                }
            }

            //private int laneChange;
            NodeList roadMarkList = laneElement.getElementsByTagName("roadMark");
            Element roadMarkElement = (Element) roadMarkList.item(0);
            String laneChange = roadMarkElement.getAttribute("laneChange");
            lane.setLaneChange(laneChangeType.getOrDefault(laneChange, 0));

            // width
            NodeList widthList = laneElement.getElementsByTagName("width");
            Element widthElement = (Element) widthList.item(0);
            String width = "0.0";
            if (widthElement != null) { // 不是所有的lane都有width，比如center（中心线）没有
                width = widthElement.getAttribute("a");
            }
            lane.setWidth(Double.parseDouble(width));

            laneSectionLanes.add(lane);
            lanes.add(lane);
        }
        laneSection.setLanesIndex(lanesIndex);
        laneSection.setLanes(laneSectionLanes);
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.7 解析交叉口

private static void parseJunction(NodeList junctionList, List<Junction> junctions, List<Connection> connections, List<LaneLink> laneLinks) {
    try {
        for (int i = 0; i < junctionList.getLength(); i++) {
            Element junctionElement = (Element) junctionList.item(i);

            Junction junction = new Junction();
            // int elementType;
            junction.setElementType(ElementType.JUNCTION.getValue());
            // int junctionId;
            int junctionId = Integer.parseInt(junctionElement.getAttribute("id"));

            junction.setIndex(junctionIndex);
            junctionMap.put(junctionId, junctionIndex++);

            // connections;
            NodeList connectionList = junctionElement.getElementsByTagName("connection");
            parseConnection(connectionList, junction, connections, laneLinks);

            junctions.add(junction);
        }
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.8 解析连接关系

private static void parseConnection(NodeList connectionList, Junction junction, List<Connection> connections, List<LaneLink> laneLinks) {
    try {
        List<Connection> junctionConnections = new ArrayList<>();
        List<Integer> connectionsIndex = new ArrayList<>();
        for (int i = 0; i < connectionList.getLength(); i++) {
            Element connectionElement = (Element) connectionList.item(i);

            Connection connection = new Connection();
            int incomingRoadId = Integer.parseInt(connectionElement.getAttribute("incomingRoad"));
            int connectingRoadId = Integer.parseInt(connectionElement.getAttribute("connectingRoad"));

            connection.setIncomingRoadId(incomingRoadId);
            connection.setConnectingRoadId(connectingRoadId);

            connection.setIndex(connectionIndex);
            connectionsIndex.add(connectionIndex++); // 上级junction中connection索引

            // laneLinks
            NodeList laneLinkList = connectionElement.getElementsByTagName("laneLink");
            parseLaneLink(laneLinkList, connection, laneLinks);

            connections.add(connection);
            junctionConnections.add(connection);
        }
        junction.setConnectionsIndex(connectionsIndex);
        junction.setConnections(junctionConnections);
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.9 解析车道连接

private static void parseLaneLink(NodeList laneLinkList, Connection connection, List<LaneLink> laneLinks) {
    try {
        List<LaneLink> connectionLaneLinks = new ArrayList<>();
        List<Integer> laneLinksIndex = new ArrayList<>();
        for (int i = 0; i < laneLinkList.getLength(); i++) {
            Element laneLinkElement = (Element) laneLinkList.item(i);
            LaneLink laneLink = new LaneLink();

            laneLink.setFrom(Integer.parseInt(laneLinkElement.getAttribute("from")));
            laneLink.setTo(Integer.parseInt(laneLinkElement.getAttribute("to")));

            laneLink.setIndex(laneLinkIndex);
            laneLinksIndex.add(laneLinkIndex++); // 上级laneLinks索引

            connectionLaneLinks.add(laneLink);
            laneLinks.add(laneLink);
        }
        connection.setLaneLinksIndex(laneLinksIndex);
        connection.setLaneLinks(connectionLaneLinks);
    } catch (Exception e) {
        e.printStackTrace();
    }
}

2.10 初始化各结构的索引

private static void initIndex(List<Road> roads, List<LaneSection> laneSections, List<Lane> lanes,
                              List<Junction> junctions, List<Connection> connections, List<LaneLink> laneLinks) {
    // road: 需要初始化junctionIndex, predecessorIndex, successorIndex
    initRoad(roads);
    // lane: 需要初始化predecessorIndex, successorIndex
    initLane(lanes, laneSections, roads);
    // connection: 需要初始化incomingRoadIndex, connectionRoadIndex; direction
    initConnection(connections, junctions, roads);
}

2.11 初始化Road

private static void initRoad(List<Road> roads) {
    // road: junctionIndex, predecessorIndex, successorIndex
    for (Road road : roads) {
        // junctionIndex
        road.setJunctionIndex(junctionMap.getOrDefault(road.getJunctionId(), -1));
        // predecessorIndex
        if (road.getPredecessorElementType() == 1) { // road
            road.setPredecessorIndex(roadMap.getOrDefault(road.getPredecessorId(), -1));
        } else if (road.getPredecessorElementType() == 4) { // junction
            road.setPredecessorIndex(junctionMap.getOrDefault(road.getPredecessorId(), -1));
        } else {
            road.setPredecessorIndex(-1);
        }
        // successorIndex
        if (road.getSuccessorElementType() == 1) { // road
            road.setSuccessorIndex(roadMap.getOrDefault(road.getSuccessorId(), -1));
        } else if (road.getSuccessorElementType() == 4) { // junction
            road.setSuccessorIndex(junctionMap.getOrDefault(road.getSuccessorId(), -1));
        } else {
            road.setSuccessorIndex(-1);
        }
    }
}

2.12 初始化车道

private static void initLane(List<Lane> lanes, List<LaneSection> laneSections, List<Road> roads) {
    log.warn("Lane的前驱后继索引初始化尚未完成：跨road部分");
    for (Lane lane : lanes) {
        Road currentRoad = roads.get(lane.getRoadIndex());
        LaneSection currentLaneSection = laneSections.get(lane.getLaneSectionIndex());

        /*
                前驱：
                1 当前laneSection不是当前road的第一个车道段，那么
                - 当前laneSection的index-1，即前一个laneSection，找到对应的predecessorLaneId即可
                2 当前laneSection是当前road的第一个车道段，那么
                - 找到当前road的前驱road，根据两条road的连接方式判断predecessorLaneId属于前驱road的第一个还是最后一个车道段
            */
        if (lane.getPredecessorLaneId() != 0) {
            LaneSection preLaneSection;
            if (currentLaneSection.getStartPosition() != 0.0) { // 不是第一个车道段
                preLaneSection = laneSections.get(currentLaneSection.getIndex() - 1);
            } else { //是第一个车道段
                if (StrUtil.isNotBlank(String.valueOf(currentRoad.getPredecessorId()))) {
                    if (ObjectUtil.isNotEmpty(roadMap.get(currentRoad.getPredecessorId()))) {
                        Road preRoad = roads.get(roadMap.get(currentRoad.getPredecessorId())); // 如果是Junction？
                        int connectType = 0; // TODO: 两个road的连接方式，影响着前一个laneSection是preRoad的第一个还是最后一个
                        if (connectType == 0) { // 第一个
                            preLaneSection = preRoad.getLaneSections().get(0);
                        } else { // 最后一个
                            int length = preRoad.getLaneSections().size();
                            preLaneSection = preRoad.getLaneSections().get(length - 1);
                        }
                        updatePreLaneIndex(lane, preLaneSection);
                    }
                }
            }

        }

        /*
                后继：
                1 当前laneSection不是当前road的最后一个车道段，那么
                - 当前laneSection的index+1，即后一个laneSection，找到对应的successorLaneId即可
                2 当前laneSection是当前road的最后一个车道段，那么
                - 找到当前road的后继road，根据两条road的连接方式判断successorLaneId属于后继road的第一个还是最后一个车道段
             */
        if (lane.getSuccessorLaneId() != 0) {
            LaneSection sucLaneSection;
            // 最后一个车道段
            int lastIndex = currentRoad.getLaneSections().size() - 1;
            LaneSection lastLaneSection = currentRoad.getLaneSections().get(lastIndex);

            if (currentLaneSection.getStartPosition() != lastLaneSection.getStartPosition()) { // 不是最后一个车道段
                sucLaneSection = laneSections.get(currentLaneSection.getIndex() + 1);
            } else { //是最后一个车道段
                if (StrUtil.isNotBlank(String.valueOf(currentRoad.getSuccessorId()))) {
                    if (ObjectUtil.isNotEmpty(roadMap.get(currentRoad.getSuccessorId()))) {
                        Road sucRoad = roads.get(roadMap.get(currentRoad.getSuccessorId()));
                        int connectType = 0; // TODO: 两个road的连接方式，影响着前一个laneSection是preRoad的第一个还是最后一个
                        if (connectType == 0) { // 第一个
                            sucLaneSection = sucRoad.getLaneSections().get(0);
                        } else { // 最后一个
                            int length = sucRoad.getLaneSections().size();
                            sucLaneSection = sucRoad.getLaneSections().get(length - 1);
                        }
                        updateSucLaneIndex(lane, sucLaneSection);
                    }
                }
            }
        }

    }
}

抽取出来的一个方法，通过前驱laneSection更新lane的前驱：

private static void updatePreLaneIndex(Lane lane, LaneSection preLaneSection) {
    // 更新lane
    for (Lane preLane : preLaneSection.getLanes()) {
        if (preLane.getLaneId() == lane.getPredecessorLaneId()) {
            lane.setPredecessorSingleId(preLane.getSingleId());
            lane.setPredecessorIndex(preLane.getIndex());
            break;
        }
    }
}

抽取出来的一个方法，通过后继laneSection更新lane的后继：

private static void updateSucLaneIndex(Lane lane, LaneSection sucLaneSection) {
    for (Lane preLane : sucLaneSection.getLanes()) {
        if (preLane.getLaneId() == lane.getPredecessorLaneId()) {
            lane.setSuccessorSingleId(preLane.getSingleId());
            lane.setSuccessorIndex(preLane.getIndex());
            break;
        }
    }
}

2.13 初始化连接

private static void initConnection(List<Connection> connections, List<Junction> junctions, List<Road> roads) {
    // connection: incomingRoadIndex, connectionRoadIndex
    for (Connection connection : connections) {
        connection.setIncomingRoadIndex(roadMap.getOrDefault(connection.getIncomingRoadId(), -1));
        connection.setConnectingRoadIndex(roadMap.getOrDefault(connection.getConnectingRoadId(), -1));
    }

    // 设置direction
    for (Road road : roads) {
        if (road.getJunctionId() != -1) {
            int direction = 1; // 1左转 2直行 3右转 其他都不正常忽略掉
            for (Junction junction : junctions) {
                for (Connection connection : junction.getConnections()) {
                    if (connection.getConnectingRoadId() == road.getRoadId()) { // 作为连接路
                        connection.setDirection(direction++);
                    }
                }
            }
        }
    }
}