test_opendrive.cpp

Go to the documentation of this file.

// Copyright (c) 2026 Computer Vision Center (CVC) at the Universitat Autonoma
// de Barcelona (UAB).
//
// This work is licensed under the terms of the MIT license.
// For a copy, see <https://opensource.org/licenses/MIT>.
 
#include "test.h"
#include "OpenDrive.h"
#include "Random.h"
 
#include <carla/StopWatch.h>
#include <carla/geom/Location.h>
#include <carla/geom/Math.h>
#include <carla/opendrive/OpenDriveParser.h>
#include <carla/road/MapBuilder.h>
#include <carla/road/element/RoadInfoElevation.h>
#include <carla/road/element/RoadInfoGeometry.h>
#include <carla/road/element/RoadInfoMarkRecord.h>
#include <carla/road/element/RoadInfoVisitor.h>
 
#include <third-party/pugixml/pugixml.hpp>
 
#include <fstream>
#include <string>
 
using namespace carla::road;
using namespace carla::road::element;
using namespace carla::geom;
using namespace carla::opendrive;
using namespace util;
 
const std::string BASE_PATH = LIBCARLA_TEST_CONTENT_FOLDER "/OpenDrive/";
 
// Road Elevation
static void test_road_elevation(const pugi::xml_document &xml, std::optional<Map>& map) {
  pugi::xml_node open_drive_node = xml.child("OpenDRIVE");
 
  for (pugi::xml_node road_node : open_drive_node.children("road")) {
    RoadId road_id = road_node.attribute("id").as_uint();
    auto elevation_profile_nodes = road_node.children("elevationProfile");
 
    for (pugi::xml_node elevation_profile_node : elevation_profile_nodes) {
      auto total_elevations = 0;
      auto elevation_nodes = elevation_profile_node.children("elevation");
      auto total_elevation_parser = std::distance(elevation_nodes.begin(), elevation_nodes.end());
 
      for (pugi::xml_node elevation_node : elevation_nodes) {
        float s = elevation_node.attribute("s").as_float();
        const auto elevation = map->GetMap().GetRoad(road_id).GetInfo<RoadInfoElevation>(s);
        if (elevation != nullptr)
          ++total_elevations;
      }
      ASSERT_EQ(total_elevations, total_elevation_parser);
    }
  }
}
 
// Geometry
static void test_geometry(const pugi::xml_document &xml, std::optional<Map>& map) {
  pugi::xml_node open_drive_node = xml.child("OpenDRIVE");
 
  for (pugi::xml_node road_node : open_drive_node.children("road")) {
    RoadId road_id = road_node.attribute("id").as_uint();
 
    for (pugi::xml_node plan_view_nodes : road_node.children("planView")) {
      auto geometries_parser = plan_view_nodes.children("geometry");
      auto total_geometries_parser = std::distance(geometries_parser.begin(), geometries_parser.end());
      auto total_geometries = 0;
      for (pugi::xml_node geometry_node : plan_view_nodes.children("geometry")){
        float s = geometry_node.attribute("s").as_float();
        auto geometry = map->GetMap().GetRoad(road_id).GetInfo<RoadInfoGeometry>(s);
        if (geometry != nullptr)
          ++total_geometries;
      }
      ASSERT_EQ(total_geometries, total_geometries_parser);
    }
  }
}
 
// Road test
static auto get_total_road_marks(
    pugi::xml_object_range<pugi::xml_named_node_iterator> &lane_nodes,
    LaneSection& lane_section) {
  constexpr auto error = 1e-5;
  auto total_road_mark = 0;
  auto total_road_mark_parser = 0;
  for (pugi::xml_node lane_node : lane_nodes) {
    // Check Road Mark
    auto road_mark_nodes = lane_node.children("roadMark");
    total_road_mark_parser += std::distance(road_mark_nodes.begin(), road_mark_nodes.end());
 
    const int lane_id = lane_node.attribute("id").as_int();
    Lane* lane = nullptr;
    lane = lane_section.GetLane(lane_id);
    EXPECT_NE(lane, nullptr);
 
    // <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
    for (pugi::xml_node road_mark_node : road_mark_nodes) {
      const auto s_offset = road_mark_node.attribute("sOffset").as_double();
      const auto type = road_mark_node.attribute("type").as_string();
      const auto material = road_mark_node.attribute("material").as_string();
      const auto color = road_mark_node.attribute("color").as_string();
      const auto road_mark = lane->GetInfo<RoadInfoMarkRecord>(lane->GetDistance() + s_offset);
      if (road_mark != nullptr) {
        EXPECT_NEAR(lane->GetDistance() + s_offset, road_mark->GetDistance(), error);
        EXPECT_EQ(type, road_mark->GetType());
        EXPECT_EQ(material, road_mark->GetMaterial());
        EXPECT_EQ(color, road_mark->GetColor());
        ++total_road_mark;
      }
    }
  }
  return std::make_pair(total_road_mark, total_road_mark_parser);
}
 
static void test_roads(const pugi::xml_document &xml, std::optional<Map>& map) {
  pugi::xml_node open_drive_node = xml.child("OpenDRIVE");
 
  // Check total Roads
  auto roads_parser = open_drive_node.children("road");
  auto total_roads_parser = std::distance(roads_parser.begin(), roads_parser.end());
  auto total_roads = map->GetMap().GetRoads().size();
  ASSERT_EQ(total_roads, total_roads_parser);
 
  for (pugi::xml_node road_node : roads_parser) {
    RoadId road_id = road_node.attribute("id").as_uint();
 
    for (pugi::xml_node lanes_node : road_node.children("lanes")) {
 
      // Check total Lane Sections
      auto lane_sections_parser = lanes_node.children("laneSection");
      auto total_lane_sections_parser = std::distance(lane_sections_parser.begin(), lane_sections_parser.end());
      auto total_lane_sections = map->GetMap().GetRoad(road_id).GetLaneSections().size();
      ASSERT_EQ(total_lane_sections, total_lane_sections_parser);
 
      for (pugi::xml_node lane_section_node : lane_sections_parser) {
 
        // Check total Lanes
        const double s = lane_section_node.attribute("s").as_double();
        auto lane_section = map->GetMap().GetRoad(road_id).GetLaneSectionsAt(s);
        size_t total_lanes = 0u;
        for (auto it = lane_section.begin(); it != lane_section.end(); ++it) {
          total_lanes = it->GetLanes().size();
        }
        auto left_nodes = lane_section_node.child("left").children("lane");
        auto center_nodes = lane_section_node.child("center").children("lane");
        auto right_nodes = lane_section_node.child("right").children("lane");
        auto total_lanes_parser = std::distance(left_nodes.begin(), left_nodes.end());
        total_lanes_parser += std::distance(right_nodes.begin(), right_nodes.end());
        total_lanes_parser += std::distance(center_nodes.begin(), center_nodes.end());
 
        ASSERT_EQ(total_lanes, total_lanes_parser);
 
 
        auto total_road_mark = 0;
        auto total_road_mark_parser = 0;
        for (auto it = lane_section.begin(); it != lane_section.end(); ++it) {
          auto total_left = get_total_road_marks(left_nodes, *it);
          auto total_center = get_total_road_marks(center_nodes, *it);
          auto total_right = get_total_road_marks(right_nodes, *it);
          total_road_mark = total_left.first + total_center.first + total_right.first;
          total_road_mark_parser = total_left.first + total_center.first + total_right.first;
        }
        ASSERT_EQ(total_road_mark, total_road_mark_parser);
      }
    }
  }
}
 
// Junctions
static void test_junctions(const pugi::xml_document &xml, std::optional<Map>& map) {
  pugi::xml_node open_drive_node = xml.child("OpenDRIVE");
 
  // Check total number of junctions
  auto& junctions = map->GetMap().GetJunctions();
  auto total_junctions_parser = std::distance(open_drive_node.children("junction").begin(), open_drive_node.children("junction").end());
 
  ASSERT_EQ(junctions.size(), total_junctions_parser);
 
  for (pugi::xml_node junction_node : open_drive_node.children("junction")) {
    // Check total number of connections
    auto total_connections_parser = std::distance(junction_node.children("connection").begin(), junction_node.children("connection").end());
 
    JuncId junction_id = junction_node.attribute("id").as_int();
    auto& junction = junctions.find(junction_id)->second;
 
    auto& connections = junction.GetConnections();
 
    ASSERT_EQ(connections.size(), total_connections_parser);
 
    for (pugi::xml_node connection_node : junction_node.children("connection")) {
      auto total_lane_links_parser = std::distance(connection_node.children("laneLink").begin(), connection_node.children("laneLink").end());
 
      ConId connection_id = connection_node.attribute("id").as_uint();
      auto& connection = connections.find(connection_id)->second;
 
    auto& lane_links = connection.lane_links;
 
    ASSERT_EQ(lane_links.size(), total_lane_links_parser);
 
    }
  }
}
 
static void test_road_links(std::optional<Map>& map) {
 
  // process all roads, sections and lanes
  for (auto &road : map->GetMap().GetRoads()) {
    for (auto &section : road.second.GetLaneSections()) {
      for (auto &lane : section.GetLanes()) {
        // check all nexts
        for (auto link : lane.second.GetNextLanes()) {
          ASSERT_TRUE(link != nullptr);
        }
        // check all prevs
        for (auto link : lane.second.GetPreviousLanes()) {
          ASSERT_TRUE(link != nullptr);
        }
      }
    }
  }
}
 
// A valid OpenDRIVE file may legitimately contain no road at all: CARLA ships
// EmptyMap.xodr, a header-only file, so that world.get_map() works on the empty
// sandbox map. Geometry tests have nothing to assert on such a map.
static bool IsRoadless(carla::road::Map &map) {
  return map.GetMap().GetRoads().empty();
}
 
TEST(road, parse_files) {
  for (const auto &file : util::OpenDrive::GetAvailableFiles()) {
    // std::cerr << file << std::endl;
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map);
    // print_roads(map, file);
  }
}
 
TEST(road, parse_road_links) {
  for (const auto &file : util::OpenDrive::GetAvailableFiles()) {
    // std::cerr << file << std::endl;
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map);
    test_road_links(map);
  }
}
 
TEST(road, parse_junctions) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map.has_value());
 
    const std::string full_path = BASE_PATH + file;
    pugi::xml_document xml;
 
    pugi::xml_parse_result result = xml.load_file( full_path.c_str());
    ASSERT_TRUE(result);
 
    test_junctions(xml, map);
  }
 
}
 
TEST(road, parse_road) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map.has_value());
 
    const std::string full_path = BASE_PATH + file;
    pugi::xml_document xml;
    pugi::xml_parse_result result = xml.load_file( full_path.c_str());
    ASSERT_TRUE(result);
 
    test_roads(xml, map);
  }
 
}
 
TEST(road, parse_road_elevation) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map.has_value());
 
    const std::string full_path = BASE_PATH + file;
    pugi::xml_document xml;
    pugi::xml_parse_result result = xml.load_file( full_path.c_str());
    ASSERT_TRUE(result);
 
    test_road_elevation(xml, map);
  }
 
}
 
TEST(road, parse_geometry) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    auto map = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(map.has_value());
 
    const std::string full_path = BASE_PATH + file;
    pugi::xml_document xml;
    pugi::xml_parse_result result = xml.load_file( full_path.c_str());
    ASSERT_TRUE(result);
 
    test_geometry(xml, map);
  }
 
}
 
TEST(road, iterate_waypoints) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    carla::logging::log("Parsing", file);
    carla::StopWatch stop_watch;
    auto m = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(m.has_value());
    auto &map = *m;
    if (IsRoadless(map)) {
      carla::logging::log("  no roads, skipping:", file);
      continue;
    }
    const auto topology = map.GenerateTopology();
    ASSERT_FALSE(topology.empty());
    auto count = 0u;
    auto waypoints = map.GenerateWaypoints(0.5);
    ASSERT_FALSE(waypoints.empty());
    Random::Shuffle(waypoints);
    const auto number_of_waypoints_to_explore =
        std::min<size_t>(2000u, waypoints.size());
    for (auto i = 0u; i < number_of_waypoints_to_explore; ++i) {
      auto wp = waypoints[i];
      map.ComputeTransform(wp);
      if (i != 0u) {
        ASSERT_NE(wp, waypoints[0u]);
      }
      for (auto &&successor : map.GetSuccessors(wp)) {
        ASSERT_TRUE(
            successor.road_id != wp.road_id ||
            successor.section_id != wp.section_id ||
            successor.lane_id != wp.lane_id ||
            successor.s != wp.s);
      }
      auto origin = wp;
      for (auto j = 0u; j < 200u; ++j) {
        auto next_wps = map.GetNext(origin, Random::Uniform(0.0001, 150.0));
        if (next_wps.empty()) {
          break;
        }
        const auto number_of_next_wps_to_explore =
            std::min<size_t>(10u, next_wps.size());
        Random::Shuffle(next_wps);
        for (auto k = 0u; k < number_of_next_wps_to_explore; ++k) {
          auto next = next_wps[k];
          ++count;
          ASSERT_TRUE(
              next.road_id != wp.road_id ||
              next.section_id != wp.section_id ||
              next.lane_id != wp.lane_id ||
              next.s != wp.s);
          auto right = map.GetRight(next);
          if (right.has_value()) {
            ASSERT_EQ(right->road_id, next.road_id);
            ASSERT_EQ(right->section_id, next.section_id);
            ASSERT_NE(right->lane_id, next.lane_id);
            ASSERT_EQ(right->s, next.s);
          }
          auto left = map.GetLeft(next);
          if (left.has_value()) {
            ASSERT_EQ(left->road_id, next.road_id);
            ASSERT_EQ(left->section_id, next.section_id);
            ASSERT_NE(left->lane_id, next.lane_id);
            ASSERT_EQ(left->s, next.s);
          }
        }
        origin = next_wps[0u];
      }
    }
    ASSERT_GT(count, 0u);
    float seconds = 1e-3f * static_cast<float>(stop_watch.GetElapsedTime());
    carla::logging::log(file, "done in", seconds, "seconds.");
  }
}
 
TEST(road, get_waypoint) {
  for (const auto& file : util::OpenDrive::GetAvailableFiles()) {
    carla::logging::log("Parsing", file);
    carla::StopWatch stop_watch;
    auto m = OpenDriveParser::Load(util::OpenDrive::Load(file));
    ASSERT_TRUE(m.has_value());
    auto &map = *m;
    if (IsRoadless(map)) {
      carla::logging::log("  no roads, skipping:", file);
      continue;
    }
    for (auto i = 0u; i < 10'000u; ++i) {
      const auto location = Random::Location(-500.0f, 500.0f);
      auto owp = map.GetClosestWaypointOnRoad(location);
      ASSERT_TRUE(owp.has_value());
      auto &wp = *owp;
      for (auto &next : map.GetNext(wp, 0.5)) {
        ASSERT_TRUE(
            next.road_id != wp.road_id ||
            next.section_id != wp.section_id ||
            next.lane_id != wp.lane_id ||
            next.s != wp.s);
      }
      auto left = map.GetLeft(wp);
      if (left.has_value()) {
        ASSERT_EQ(left->road_id, wp.road_id);
        ASSERT_EQ(left->section_id, wp.section_id);
        ASSERT_NE(left->lane_id, wp.lane_id);
        ASSERT_EQ(left->s, wp.s);
      }
      auto right = map.GetRight(wp);
      if (right.has_value()) {
        ASSERT_EQ(right->road_id, wp.road_id);
        ASSERT_EQ(right->section_id, wp.section_id);
        ASSERT_NE(right->lane_id, wp.lane_id);
        ASSERT_EQ(right->s, wp.s);
      }
    }
    float seconds = 1e-3f * static_cast<float>(stop_watch.GetElapsedTime());
    carla::logging::log(file, "done in", seconds, "seconds.");
  }
}
 
// Minimal OpenDRIVE with a single road (id=1) carrying one stencil <object>.
// The stencil object is named "Stencil_Arrow" so the parser keys on the
// "Stencil_" prefix while the existing "Stencil_STOP" stop-line branch is
// left untouched.
static constexpr const char* kStencilXodr = R"(<?xml version="1.0" encoding="UTF-8"?>
<OpenDRIVE>
    <header revMajor="1" revMinor="4" name="" version="1" date="2026-01-01T00:00:00" north="0.0" south="0.0" east="0.0" west="0.0" vendor="CARLA"/>
    <road name="Road 1" length="2.1500000000000000e+1" id="1" junction="-1">
        <type s="0.0000000000000000e+0" type="town">
            <speed max="35" unit="mph"/>
        </type>
        <planView>
            <geometry s="0.0000000000000000e+0" x="0.0000000000000000e+0" y="0.0000000000000000e+0" hdg="0.0000000000000000e+0" length="2.1500000000000000e+1">
                <line/>
            </geometry>
        </planView>
        <elevationProfile>
            <elevation s="0.0000000000000000e+0" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
        </elevationProfile>
        <lanes>
            <laneOffset s="0.0000000000000000e+0" a="0.0000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
            <laneSection s="0.0000000000000000e+0" singleSide="false">
                <center>
                    <lane id="0" type="none" level="false">
                        <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
                    </lane>
                </center>
                <right>
                    <lane id="-1" type="driving" level="false">
                        <width sOffset="0.0000000000000000e+0" a="3.5000000000000000e+0" b="0.0000000000000000e+0" c="0.0000000000000000e+0" d="0.0000000000000000e+0"/>
                        <roadMark sOffset="0.0000000000000000e+0" type="none" material="standard" color="white" laneChange="none"/>
                        <speed sOffset="0.0000000000000000e+0" max="3.5000000000000000e+1" unit="mph"/>
                    </lane>
                </right>
            </laneSection>
        </lanes>
        <objects>
            <object id="100" name="Stencil_Arrow" s="5.0000000000000000e+0" t="0.0000000000000000e+0" zOffset="0.0000000000000000e+0" hdg="0.0000000000000000e+0" roll="0.0000000000000000e+0" pitch="0.0000000000000000e+0" orientation="none" type="stencil" length="2.0000000000000000e+0" width="1.0000000000000000e+0"/>
        </objects>
    </road>
</OpenDRIVE>
)";
 
TEST(road, stencil_parsing) {
  std::string xodr = kStencilXodr;
  auto map = OpenDriveParser::Load(xodr);
  ASSERT_TRUE(map.has_value());
  auto stencils = map->GetAllStencilReferences();
  ASSERT_EQ(stencils.size(), 1u);
  EXPECT_EQ(stencils[0]->GetRoadId(), 1u);
 
  // The build finalization must populate the map-level stencil container and
  // link each RoadInfoStencil to the corresponding Stencil.
  const auto &map_stencils = map->GetStencils();
  ASSERT_EQ(map_stencils.size(), 1u);
  const auto stencil_id = stencils[0]->GetStencilId();
  auto it = map_stencils.find(stencil_id);
  ASSERT_NE(it, map_stencils.end());
  EXPECT_EQ(stencils[0]->GetStencil(), it->second.get());
 
  // A search that runs past the lane end must still return the stencils
  // before the lane end.
  const Waypoint start{1u, 0u, -1, 0.0};
  auto found = map->GetStencilsInDistance(start, 30.0, false);
  ASSERT_EQ(found.size(), 1u);
  EXPECT_DOUBLE_EQ(found[0].accumulated_s, 5.0);
}