diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..a90eb86 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "pybind11"] + path = pybind11 + url = https://github.com/pybind/pybind11.git diff --git a/CMakeLists.txt b/CMakeLists.txt index 23e279e..8dca612 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -7,7 +7,9 @@ include(GNUInstallDirs) include(CMakePackageConfigHelpers) option(OPENDRIVE_BUILD_TESTS "Build OpenDrive tests" OFF) +option(BUILD_PYTHON_BINDINGS "Build Python bindings for libOpenDRIVE" OFF) +# Try to use an installed pugixml first; otherwise fetch it find_package(pugixml QUIET CONFIG) if(NOT TARGET pugixml::pugixml) if(TARGET pugixml) @@ -23,6 +25,7 @@ if(NOT TARGET pugixml::pugixml) endif() endif() +# Define libOpenDRIVE sources set(SOURCES src/Geometries/Arc.cpp src/Geometries/CubicSpline.cpp @@ -45,6 +48,7 @@ set(SOURCES src/RoutingGraph.cpp ) +# Build libOpenDRIVE add_library(OpenDrive ${SOURCES}) add_library(OpenDrive::OpenDrive ALIAS OpenDrive) set_target_properties(OpenDrive PROPERTIES @@ -52,6 +56,7 @@ set_target_properties(OpenDrive PROPERTIES CXX_STANDARD_REQUIRED ON POSITION_INDEPENDENT_CODE ON ) + if(MSVC) target_compile_options(OpenDrive PRIVATE /W3 /EHsc) target_compile_definitions(OpenDrive PRIVATE _USE_MATH_DEFINES) @@ -67,14 +72,20 @@ target_include_directories( $ ) +# Tests (optional) if(OPENDRIVE_BUILD_TESTS) include(CTest) - FetchContent_Declare( - Catch2 - GIT_REPOSITORY https://github.com/catchorg/Catch2.git - GIT_TAG v3.4.0 - ) - FetchContent_MakeAvailable(Catch2) + + # Prefer an installed Catch2, fallback to fetching it + find_package(Catch2 QUIET CONFIG) + if(NOT TARGET Catch2::Catch2WithMain) + FetchContent_Declare( + Catch2 + GIT_REPOSITORY https://github.com/catchorg/Catch2.git + GIT_TAG v3.4.0 + ) + FetchContent_MakeAvailable(Catch2) + endif() include(Catch) add_executable(tests tests/test.cpp) @@ -86,6 +97,49 @@ if(OPENDRIVE_BUILD_TESTS) catch_discover_tests(tests WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}/tests) endif() +# Add Python bindings (optional, controlled by option) +if(BUILD_PYTHON_BINDINGS) + find_package(Python3 COMPONENTS Interpreter Development REQUIRED) + find_package(pybind11 CONFIG REQUIRED) + + # Define binding sources + set(BINDING_SOURCES + bindings/bindings.cpp + bindings/opendrivemap.cpp + bindings/junction.cpp + bindings/lane.cpp + bindings/lanesection.cpp + bindings/road.cpp + bindings/refline.cpp + bindings/mesh.cpp + bindings/routinggraph.cpp + bindings/roadmark.cpp + bindings/roadnetworkmesh.cpp + bindings/roadobject.cpp + bindings/lanevalidityrecord.cpp + bindings/roadsignal.cpp + bindings/math.cpp + bindings/xml_node.cpp + # Add other binding files as needed + ) + + # Create Python module + pybind11_add_module(open_drive ${BINDING_SOURCES}) + target_link_libraries(open_drive PRIVATE OpenDrive) + + # Public include dir for the module (OpenDrive already exposes its include dir) + target_include_directories( + open_drive PRIVATE + $ + ) + + # Install Python module + install(TARGETS open_drive + LIBRARY DESTINATION ${Python3_SITELIB} + ) +endif() + +# Installation install( TARGETS OpenDrive EXPORT OpenDriveTargets @@ -120,4 +174,4 @@ install(FILES "${CMAKE_CURRENT_BINARY_DIR}/OpenDriveConfig.cmake" "${CMAKE_CURRENT_BINARY_DIR}/OpenDriveConfigVersion.cmake" DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/OpenDrive -) +) \ No newline at end of file diff --git a/README.md b/README.md index 885284c..a54dc3c 100644 --- a/README.md +++ b/README.md @@ -50,6 +50,46 @@ If requiring a shared library, use: cmake -DBUILD_SHARED_LIBS=ON .. ``` +### Python Bindings with pybind11 + +To build the Python bindings using pybind11 and enable installation via pip in editable mode (`pip install -e .`), follow these steps: + +1. **Create and enter the build directory:** + + ```bash + mkdir build + cd build + +2. **Configure the build with Python bindings enabled, specifying your Python interpreter:** + + ``` + cmake -DBUILD_SHARED_LIBS=ON -DBUILD_PYTHON_BINDINGS=ON -DPython3_EXECUTABLE=$(which python) .. + ``` + +3. **Build the project:** + ``` + make + ``` + +4. **Install the Python package in editable mode:** + From the root directory of the repository (not inside `build`), run: + + ``` + pip install -e . + ``` + This command uses the generated Python bindings and sets up the package in your environment so that changes to the source code reflect immediately without reinstalling. + +5. **Import and use the package in Python:** +After installation, you can import the opendrive module in Python: + +``` +import opendrive + +# Example: List available attributes +print(dir(opendrive)) +``` +This verifies the bindings are available and provides an overview of the exposed classes and functions. + ## Viewer Check out the viewer at [odrviewer.io](https://odrviewer.io) which uses this library. Use the [odrviewer project](https://github.com/pageldev/odrviewer) to post issues and feature requests for odrviewer.io. diff --git a/bindings/bindings.cpp b/bindings/bindings.cpp new file mode 100644 index 0000000..be5e978 --- /dev/null +++ b/bindings/bindings.cpp @@ -0,0 +1,37 @@ +#include "bindings.h" +#include +#include +#include +#include // For RoadLink::ContactPoint + +namespace py = pybind11; + +PYBIND11_MODULE(opendrive, m) { + m.doc() = "Python bindings for libOpenDRIVE"; + + // RoadLink depends on XmlNode + init_xml_node(m); + + // Bind ContactPoint enum + py::enum_(m, "ContactPoint") + .value("None", odr::RoadLink::ContactPoint::ContactPoint_None) + .value("Start", odr::RoadLink::ContactPoint::ContactPoint_Start) + .value("End", odr::RoadLink::ContactPoint::ContactPoint_End) + .export_values(); + + // Initialize other modules + init_opendrivemap(m); + init_mesh(m); + init_junction(m); + init_lane(m); + init_lanesection(m); + init_road(m); + init_refline(m); + init_routinggraph(m); + init_roadmark(m); + init_roadnetworkmesh(m); + init_roadobject(m); + init_lanevalidityrecord(m); + init_roadsignal(m); + init_math(m); +} diff --git a/bindings/bindings.h b/bindings/bindings.h new file mode 100644 index 0000000..2c4fde2 --- /dev/null +++ b/bindings/bindings.h @@ -0,0 +1,20 @@ +#pragma once +#include + +namespace py = pybind11; + +void init_opendrivemap(py::module_ &); +void init_mesh(py::module_ &); +void init_junction(py::module_ &); +void init_lane(py::module_ &); +void init_lanesection(py::module_ &); +void init_road(py::module_ &); +void init_routinggraph(py::module_ &); +void init_roadmark(py::module_ &); +void init_roadnetworkmesh(py::module_ &); +void init_roadobject(py::module_ &); +void init_lanevalidityrecord(py::module_ &); +void init_refline(py::module_ &); +void init_roadsignal(py::module_ &); +void init_math(py::module_ &); +void init_xml_node(py::module_ &m); diff --git a/bindings/junction.cpp b/bindings/junction.cpp new file mode 100644 index 0000000..0bfc5d7 --- /dev/null +++ b/bindings/junction.cpp @@ -0,0 +1,56 @@ +#include +#include +#include "XmlNode.h" +#include + + +namespace py = pybind11; + +void init_junction(py::module_ &m) { + // Bind JunctionLaneLink + py::class_(m, "JunctionLaneLink") + .def(py::init(), + py::arg("from"), py::arg("to"), + "Constructs a JunctionLaneLink with from and to lane IDs") + .def_readwrite("from", &odr::JunctionLaneLink::from, "Source lane ID") + .def_readwrite("to", &odr::JunctionLaneLink::to, "Destination lane ID"); + + // Bind JunctionConnection + py::class_(m, "JunctionConnection") + .def(py::init(), + py::arg("id"), py::arg("incoming_road"), py::arg("connecting_road"), py::arg("contact_point"), + "Constructs a JunctionConnection with ID, roads, and contact point") + .def_readwrite("id", &odr::JunctionConnection::id, "Connection ID") + .def_readwrite("incoming_road", &odr::JunctionConnection::incoming_road, "Incoming road ID") + .def_readwrite("connecting_road", &odr::JunctionConnection::connecting_road, "Connecting road ID") + .def_readwrite("contact_point", &odr::JunctionConnection::contact_point, "Contact point (None, Start, End)") + .def_readwrite("lane_links", &odr::JunctionConnection::lane_links, "Set of lane links"); + + // Bind JunctionPriority + py::class_(m, "JunctionPriority") + .def(py::init(), + py::arg("high"), py::arg("low"), + "Constructs a JunctionPriority with high and low priority road IDs") + .def_readwrite("high", &odr::JunctionPriority::high, "High priority road ID") + .def_readwrite("low", &odr::JunctionPriority::low, "Low priority road ID"); + + // Bind JunctionController + py::class_(m, "JunctionController") + .def(py::init(), + py::arg("id"), py::arg("type"), py::arg("sequence"), + "Constructs a JunctionController with ID, type, and sequence") + .def_readwrite("id", &odr::JunctionController::id, "Controller ID") + .def_readwrite("type", &odr::JunctionController::type, "Controller type") + .def_readwrite("sequence", &odr::JunctionController::sequence, "Controller sequence number"); + + // Bind Junction + py::class_(m, "Junction") + .def(py::init(), + py::arg("name"), py::arg("id"), + "Constructs a Junction with name and ID") + .def_readwrite("name", &odr::Junction::name, "Junction name") + .def_readwrite("id", &odr::Junction::id, "Junction ID") + .def_readwrite("id_to_connection", &odr::Junction::id_to_connection, "Map of connection IDs to JunctionConnection objects") + .def_readwrite("id_to_controller", &odr::Junction::id_to_controller, "Map of controller IDs to JunctionController objects") + .def_readwrite("priorities", &odr::Junction::priorities, "Set of JunctionPriority objects"); +} diff --git a/bindings/lane.cpp b/bindings/lane.cpp new file mode 100644 index 0000000..3346e51 --- /dev/null +++ b/bindings/lane.cpp @@ -0,0 +1,59 @@ +#include +#include +#include + +namespace py = pybind11; + +// Forward declarations for opaque types +namespace odr { +struct CubicSpline; +struct RoadMark; +struct RoadMarkGroup; +} + +void init_lane(py::module_ &m) { + // Bind HeightOffset + py::class_(m, "HeightOffset") + .def(py::init(), + py::arg("inner"), py::arg("outer"), + "Constructs a HeightOffset with inner and outer offsets") + .def_readwrite("inner", &odr::HeightOffset::inner, "Inner height offset") + .def_readwrite("outer", &odr::HeightOffset::outer, "Outer height offset"); + + // Bind LaneKey + py::class_(m, "LaneKey") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("lane_id"), + "Constructs a LaneKey with road ID, lane section s0, and lane ID") + .def_readwrite("road_id", &odr::LaneKey::road_id, "Road ID") + .def_readwrite("lanesection_s0", &odr::LaneKey::lanesection_s0, "Lane section s0") + .def_readwrite("lane_id", &odr::LaneKey::lane_id, "Lane ID") + .def("to_string", &odr::LaneKey::to_string, "Returns a string representation of the LaneKey") + .def(" __eq__", [](const odr::LaneKey& a, const odr::LaneKey& b) { return a == b; }, "Equality comparison"); + + // Bind opaque types + py::class_(m, "CubicSpline"); + + // Bind Lane + py::class_(m, "Lane") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("id"), + py::arg("level"), py::arg("type"), + "Constructs a Lane with road ID, lane section s0, ID, level, and type") + .def_readwrite("key", &odr::Lane::key, "Lane key") + .def_readwrite("id", &odr::Lane::id, "Lane ID") + .def_readwrite("level", &odr::Lane::level, "Level flag") + .def_readwrite("predecessor", &odr::Lane::predecessor, "Predecessor lane ID") + .def_readwrite("successor", &odr::Lane::successor, "Successor lane ID") + .def_readwrite("type", &odr::Lane::type, "Lane type") + .def_readwrite("lane_width", &odr::Lane::lane_width, "Cubic spline for lane width") + .def_readwrite("outer_border", &odr::Lane::outer_border, "Cubic spline for outer border") + .def_readwrite("s_to_height_offset", &odr::Lane::s_to_height_offset, + "Map of s-coordinates to height offsets") + .def_readwrite("roadmark_groups", &odr::Lane::roadmark_groups, + "Set of road mark groups") + .def("get_roadmarks", &odr::Lane::get_roadmarks, + py::arg("s_start"), py::arg("s_end"), + py::return_value_policy::move, + "Returns road marks between s_start and s_end"); +} diff --git a/bindings/lanesection.cpp b/bindings/lanesection.cpp new file mode 100644 index 0000000..7b26ead --- /dev/null +++ b/bindings/lanesection.cpp @@ -0,0 +1,30 @@ +#include +#include +#include + +namespace py = pybind11; + +void init_lanesection(py::module_ &m) { + // Bind LaneSection + py::class_(m, "LaneSection") + .def(py::init(), + py::arg("road_id"), py::arg("s0"), + "Constructs a LaneSection with road ID and start position s0") + .def_readwrite("road_id", &odr::LaneSection::road_id, "Road ID") + .def_readwrite("s0", &odr::LaneSection::s0, "Start position (s-coordinate)") + .def_readwrite("id_to_lane", &odr::LaneSection::id_to_lane, "Map of lane IDs to Lane objects") + .def("get_lanes", &odr::LaneSection::get_lanes, + py::return_value_policy::move, + "Returns a list of lanes") + .def("get_lane_id", &odr::LaneSection::get_lane_id, + py::arg("s"), py::arg("t"), + "Returns the lane ID at position (s, t), favoring the inner lane on boundaries") + .def("get_lane", py::overload_cast(&odr::LaneSection::get_lane, py::const_), + py::arg("id"), + py::return_value_policy::copy, + "Returns the lane with the given ID") + .def("get_lane", py::overload_cast(&odr::LaneSection::get_lane, py::const_), + py::arg("s"), py::arg("t"), + py::return_value_policy::copy, + "Returns the lane at position (s, t)"); +} diff --git a/bindings/lanevalidityrecord.cpp b/bindings/lanevalidityrecord.cpp new file mode 100644 index 0000000..a5a973f --- /dev/null +++ b/bindings/lanevalidityrecord.cpp @@ -0,0 +1,15 @@ +#include +#include +#include + +namespace py = pybind11; + +void init_lanevalidityrecord(py::module_ &m) { + // Bind LaneValidityRecord + py::class_(m, "LaneValidityRecord") + .def(py::init(), + py::arg("from_lane"), py::arg("to_lane"), + "Constructs a LaneValidityRecord with from_lane and to_lane") + .def_readwrite("from_lane", &odr::LaneValidityRecord::from_lane, "Starting lane ID") + .def_readwrite("to_lane", &odr::LaneValidityRecord::to_lane, "Ending lane ID"); +} diff --git a/bindings/math.cpp b/bindings/math.cpp new file mode 100644 index 0000000..0e2190a --- /dev/null +++ b/bindings/math.cpp @@ -0,0 +1,124 @@ +#include +#include +#include "Math.hpp" + +namespace py = pybind11; +using namespace odr; + +void init_math(py::module_ &m) { + // Bind Vec1D (std::array) + py::class_(m, "Vec1D") + .def(py::init([](double x) { return Vec1D{x}; })) + .def("__getitem__", [](const Vec1D& v, size_t i) { return v.at(i); }) + .def("__setitem__", [](Vec1D& v, size_t i, double value) { v.at(i) = value; }) + .def("__len__", [](const Vec1D&) { return 1; }) + .def("__repr__", [](const Vec1D& v) { return "Vec1D(" + std::to_string(v[0]) + ")"; }); + + // Bind Vec2D (std::array) + py::class_(m, "Vec2D") + .def(py::init([](double x, double y) { return Vec2D{x, y}; })) + .def_property("x", [](const Vec2D& v) { return v[0]; }, [](Vec2D& v, double x) { v[0] = x; }, "X coordinate") + .def_property("y", [](const Vec2D& v) { return v[1]; }, [](Vec2D& v, double y) { v[1] = y; }, "Y coordinate") + .def("__getitem__", [](const Vec2D& v, size_t i) { return v.at(i); }) + .def("__setitem__", [](Vec2D& v, size_t i, double value) { v.at(i) = value; }) + .def("__len__", [](const Vec2D&) { return 2; }) + .def("__repr__", [](const Vec2D& v) { + return "Vec2D(" + std::to_string(v[0]) + ", " + std::to_string(v[1]) + ")"; + }); + + // Bind Vec3D (std::array) + py::class_(m, "Vec3D") + .def(py::init([](double x, double y, double z) { return Vec3D{x, y, z}; })) + .def_property("x", [](const Vec3D& v) { return v[0]; }, [](Vec3D& v, double x) { v[0] = x; }, "X coordinate") + .def_property("y", [](const Vec3D& v) { return v[1]; }, [](Vec3D& v, double y) { v[1] = y; }, "Y coordinate") + .def_property("z", [](const Vec3D& v) { return v[2]; }, [](Vec3D& v, double z) { v[2] = z; }, "Z coordinate") + .def("__getitem__", [](const Vec3D& v, size_t i) { return v.at(i); }) + .def("__setitem__", [](Vec3D& v, size_t i, double value) { v.at(i) = value; }) + .def("__len__", [](const Vec3D&) { return 3; }) + .def("__repr__", [](const Vec3D& v) { + return "Vec3D(" + std::to_string(v[0]) + ", " + std::to_string(v[1]) + ", " + std::to_string(v[2]) + ")"; + }); + + // Bind Line3D (std::vector) + py::class_(m, "Line3D") + .def(py::init<>()) + .def("push_back", [](Line3D& line, const Vec3D& v) { line.push_back(v); }, + py::arg("v"), "Add a Vec3D to the line") + .def("clear", &Line3D::clear, "Clear all points in the line") + .def("__getitem__", [](const Line3D& line, size_t i) { return line.at(i); }) + .def("__setitem__", [](Line3D& line, size_t i, const Vec3D& v) { line.at(i) = v; }) + .def("__len__", [](const Line3D& line) { return line.size(); }) + .def("__repr__", [](const Line3D& line) { + std::string s = "Line3D(["; + for (size_t i = 0; i < line.size(); ++i) { + s += (i > 0 ? ", " : "") + std::to_string(line[i][0]) + ", " + + std::to_string(line[i][1]) + ", " + std::to_string(line[i][2]); + } + return s + "])"; + }); + + // Bind Mat3D (std::array, 3>) + py::class_(m, "Mat3D") + .def(py::init<>()) + .def("__getitem__", [](const Mat3D& m, size_t i) { return m.at(i); }) + .def("__setitem__", [](Mat3D& m, size_t i, const std::array& v) { m.at(i) = v; }) + .def("__len__", [](const Mat3D&) { return 3; }) + .def("__repr__", [](const Mat3D& m) { + std::string s = "Mat3D(["; + for (size_t i = 0; i < 3; ++i) { + if (i > 0) s += ", "; + s += "[" + std::to_string(m[i][0]) + ", " + + std::to_string(m[i][1]) + ", " + std::to_string(m[i][2]) + "]"; + } + return s + "])"; + }); + + // Bind Math functions + m.def("sign", &sign, py::arg("val"), "Return the sign of a value (-1, 0, or 1)"); + m.def("add", py::overload_cast(&add), + py::arg("a"), py::arg("b"), "Add two Vec2D vectors"); + m.def("add", py::overload_cast(&add), + py::arg("a"), py::arg("b"), "Add two Vec3D vectors"); + m.def("add", py::overload_cast(&add), + py::arg("scalar"), py::arg("a"), "Add a scalar to a Vec2D"); + m.def("add", py::overload_cast(&add), + py::arg("scalar"), py::arg("a"), "Add a scalar to a Vec3D"); + m.def("sub", py::overload_cast(&sub), + py::arg("a"), py::arg("b"), "Subtract two Vec2D vectors"); + m.def("sub", py::overload_cast(&sub), + py::arg("a"), py::arg("b"), "Subtract two Vec3D vectors"); + m.def("sub", py::overload_cast(&sub), + py::arg("scalar"), py::arg("a"), "Subtract a Vec2D from a scalar"); + m.def("sub", py::overload_cast(&sub), + py::arg("scalar"), py::arg("a"), "Subtract a Vec3D from a scalar"); + m.def("mut", py::overload_cast(&mut), + py::arg("scalar"), py::arg("a"), "Multiply a Vec2D by a scalar"); + m.def("mut", py::overload_cast(&mut), + py::arg("scalar"), py::arg("a"), "Multiply a Vec3D by a scalar"); + m.def("euclDistance", [](const Vec2D& a, const Vec2D& b) { return euclDistance(a, b); }, + py::arg("a"), py::arg("b"), "Compute Euclidean distance between two Vec2D vectors"); + m.def("euclDistance", [](const Vec3D& a, const Vec3D& b) { return euclDistance(a, b); }, + py::arg("a"), py::arg("b"), "Compute Euclidean distance between two Vec3D vectors"); + m.def("squaredNorm", py::overload_cast(&squaredNorm), + py::arg("v"), "Compute squared norm of a Vec2D"); + m.def("squaredNorm", py::overload_cast(&squaredNorm), + py::arg("v"), "Compute squared norm of a Vec3D"); + m.def("norm", py::overload_cast(&norm), + py::arg("v"), "Compute norm of a Vec2D"); + m.def("norm", py::overload_cast(&norm), + py::arg("v"), "Compute norm of a Vec3D"); + m.def("normalize", py::overload_cast(&normalize), + py::arg("v"), "Normalize a Vec2D"); + m.def("normalize", py::overload_cast(&normalize), + py::arg("v"), "Normalize a Vec3D"); + m.def("crossProduct", &crossProduct, + py::arg("a"), py::arg("b"), "Compute cross product of two Vec3D vectors"); + m.def("MatVecMultiplication", &MatVecMultiplication, + py::arg("m"), py::arg("v"), "Multiply a Mat3D by a Vec3D"); + m.def("EulerAnglesToMatrix", &EulerAnglesToMatrix, + py::arg("r_x"), py::arg("r_y"), py::arg("r_z"), "Convert Euler angles to a Mat3D rotation matrix"); +} + +PYBIND11_MODULE(opendrive_math, m) { + init_math(m); +} diff --git a/bindings/mesh.cpp b/bindings/mesh.cpp new file mode 100644 index 0000000..041d2ea --- /dev/null +++ b/bindings/mesh.cpp @@ -0,0 +1,28 @@ +#include +#include +#include "Mesh.h" + +namespace py = pybind11; +using namespace odr; + +void init_mesh(py::module_ &m) { + // Bind Mesh3D + py::class_(m, "Mesh3D") + .def(py::init<>()) + .def("add_mesh", &Mesh3D::add_mesh, + py::arg("other"), "Add another Mesh3D to this mesh") + .def("get_obj", &Mesh3D::get_obj, + "Get the mesh as an OBJ format string") + .def_readwrite("vertices", &Mesh3D::vertices, + "List of 3D vertices") + .def_readwrite("indices", &Mesh3D::indices, + "List of triangle indices") + .def_readwrite("normals", &Mesh3D::normals, + "List of 3D normals") + .def_readwrite("st_coordinates", &Mesh3D::st_coordinates, + "List of 2D texture coordinates"); +} + +PYBIND11_MODULE(mesh, m) { + init_mesh(m); +} diff --git a/bindings/opendrivemap.cpp b/bindings/opendrivemap.cpp new file mode 100644 index 0000000..1d2bc2d --- /dev/null +++ b/bindings/opendrivemap.cpp @@ -0,0 +1,56 @@ +// bindings/opendrivemap.cpp +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include // Include pugixml header for xml_parse_result + +namespace py = pybind11; + +void init_opendrivemap(py::module_ &m) { + // Bind pugi::xml_parse_result + py::class_(m, "XmlParseResult") + .def_readonly("status", &pugi::xml_parse_result::status, "Status of XML parsing") + .def_readonly("offset", &pugi::xml_parse_result::offset, "Offset of parsing error, if any") + .def_readonly("encoding", &pugi::xml_parse_result::encoding, "Encoding of the parsed document") + .def("description", &pugi::xml_parse_result::description, "Returns a description of the parsing error") + .def("__bool__", [](const pugi::xml_parse_result &result) { return result.status == pugi::xml_parse_status::status_ok; }, + "Returns true if parsing was successful"); + + // Bind OpenDriveMap + py::class_(m, "OpenDriveMap") + .def(py::init(), + py::arg("xodr_file"), + py::arg("center_map") = false, + py::arg("with_road_objects") = true, + py::arg("with_lateral_profile") = true, + py::arg("with_lane_height") = true, + py::arg("abs_z_for_for_local_road_obj_outline") = false, + py::arg("fix_spiral_edge_cases") = true, + py::arg("with_road_signals") = true, + "Constructs an OpenDriveMap from an XODR file with configuration options") + .def("get_road", &odr::OpenDriveMap::get_road, py::arg("id"), py::return_value_policy::copy, + "Returns the road with the specified ID") + .def("get_roads", &odr::OpenDriveMap::get_roads, py::return_value_policy::copy, + "Returns a list of roads") + .def("get_junction", &odr::OpenDriveMap::get_junction, py::arg("id"), py::return_value_policy::copy, + "Returns the junction with the specified ID") + .def("get_junctions", &odr::OpenDriveMap::get_junctions, py::return_value_policy::copy, + "Returns a list of junctions") + .def("get_road_network_mesh", &odr::OpenDriveMap::get_road_network_mesh, py::arg("eps"), + py::return_value_policy::move, "Returns the road network mesh") + .def("get_routing_graph", &odr::OpenDriveMap::get_routing_graph, py::return_value_policy::move, + "Returns the routing graph") + .def_readwrite("proj4", &odr::OpenDriveMap::proj4, "Projection string") + .def_readwrite("x_offs", &odr::OpenDriveMap::x_offs, "X offset") + .def_readwrite("y_offs", &odr::OpenDriveMap::y_offs, "Y offset") + .def_readonly("xodr_file", &odr::OpenDriveMap::xodr_file, "Path to the XODR file") + .def_readonly("id_to_road", &odr::OpenDriveMap::id_to_road, "Map of road IDs to Road objects") + .def_readonly("id_to_junction", &odr::OpenDriveMap::id_to_junction, "Map of junction IDs to Junction objects") + .def_readonly("xml_parse_result", &odr::OpenDriveMap::xml_parse_result, "Result of XML parsing"); +} diff --git a/bindings/refline.cpp b/bindings/refline.cpp new file mode 100644 index 0000000..02ec757 --- /dev/null +++ b/bindings/refline.cpp @@ -0,0 +1,48 @@ +#include +#include +#include "RefLine.h" + +namespace py = pybind11; +using namespace odr; + +void init_refline(py::module_ &m) { + py::class_(m, "RefLine") + .def(py::init(), + py::arg("road_id"), py::arg("length")) + .def("get_geometries", py::overload_cast<>(&RefLine::get_geometries, py::const_), + "Get a set of const RoadGeometry pointers") + .def("get_geometries", py::overload_cast<>(&RefLine::get_geometries), + "Get a set of non-const RoadGeometry pointers") + .def("get_geometry_s0", &RefLine::get_geometry_s0, + py::arg("s"), "Get the s0 value for the geometry at position s") + .def("get_geometry", py::overload_cast(&RefLine::get_geometry, py::const_), + py::arg("s"), "Get a const RoadGeometry pointer at position s") + .def("get_geometry", py::overload_cast(&RefLine::get_geometry), + py::arg("s"), "Get a non-const RoadGeometry pointer at position s") + .def("get_xyz", &RefLine::get_xyz, + py::arg("s"), "Get the 3D coordinates at position s") + .def("get_grad", &RefLine::get_grad, + py::arg("s"), "Get the gradient at position s") + .def("get_line", &RefLine::get_line, + py::arg("s_start"), py::arg("s_end"), py::arg("eps"), + "Get a 3D line from s_start to s_end with given epsilon") + .def("match", &RefLine::match, + py::arg("x"), py::arg("y"), "Match a 2D point to the reference line") + .def("approximate_linear", &RefLine::approximate_linear, + py::arg("eps"), py::arg("s_start"), py::arg("s_end"), + "Approximate the reference line linearly between s_start and s_end") + .def_readwrite("road_id", &RefLine::road_id) + .def_readwrite("length", &RefLine::length) + .def_readwrite("elevation_profile", &RefLine::elevation_profile) + .def("get_s0_to_geometry", [](const RefLine& self) { + std::map result; + for (const auto& [s0, geom_ptr] : self.s0_to_geometry) { + result[s0] = geom_ptr.get(); + } + return result; + }, "Return map of s0 to raw RoadGeometry pointers"); +} + +PYBIND11_MODULE(refline, m) { + init_refline(m); +} diff --git a/bindings/road.cpp b/bindings/road.cpp new file mode 100644 index 0000000..eeb9a29 --- /dev/null +++ b/bindings/road.cpp @@ -0,0 +1,110 @@ +// bindings/road.cpp +#include +#include +#include "XmlNode.h" +#include "Road.h" + +namespace py = pybind11; +using namespace odr; + +void init_road(py::module_ &m) { + // Bind Crossfall::Side enum + py::enum_(m, "CrossfallSide") + .value("Side_Both", Crossfall::Side::Side_Both) + .value("Side_Left", Crossfall::Side::Side_Left) + .value("Side_Right", Crossfall::Side::Side_Right) + .export_values(); + + // Bind Crossfall struct + py::class_(m, "Crossfall") + .def(py::init<>()) + .def("get_crossfall", &Crossfall::get_crossfall) + .def_readwrite("sides", &Crossfall::sides); + + // Bind RoadLink::Type enum + py::enum_(m, "RoadLinkType") + .value("Type_None", RoadLink::Type::Type_None) + .value("Type_Road", RoadLink::Type::Type_Road) + .value("Type_Junction", RoadLink::Type::Type_Junction) + .export_values(); + + // Bind RoadLink struct + py::class_>(m, "RoadLink") + .def(py::init<>()) + .def(py::init()) + .def_readwrite("id", &RoadLink::id) + .def_readwrite("type", &RoadLink::type) + .def_readwrite("contact_point", &RoadLink::contact_point); + + // Bind RoadNeighbor struct + py::class_>(m, "RoadNeighbor") + .def(py::init()) + .def_readwrite("id", &RoadNeighbor::id) + .def_readwrite("side", &RoadNeighbor::side) + .def_readwrite("direction", &RoadNeighbor::direction); + + // Bind SpeedRecord struct + py::class_>(m, "SpeedRecord") + .def(py::init()) + .def_readwrite("max", &SpeedRecord::max) + .def_readwrite("unit", &SpeedRecord::unit); + + // Bind Road class + py::class_>(m, "Road") + .def(py::init(), + py::arg("id"), py::arg("length"), py::arg("junction"), py::arg("name"), py::arg("left_hand_traffic") = false) + .def("get_lanesections", &Road::get_lanesections) + .def("get_road_objects", &Road::get_road_objects) + .def("get_road_signals", &Road::get_road_signals) + .def("get_lanesection_s0", &Road::get_lanesection_s0) + .def("get_lanesection", &Road::get_lanesection) + .def("get_lanesection_end", py::overload_cast(&Road::get_lanesection_end, py::const_)) + .def("get_lanesection_end", py::overload_cast(&Road::get_lanesection_end, py::const_)) + .def("get_lanesection_length", py::overload_cast(&Road::get_lanesection_length, py::const_)) + .def("get_lanesection_length", py::overload_cast(&Road::get_lanesection_length, py::const_)) + .def("get_xyz", &Road::get_xyz, py::arg("s"), py::arg("t"), py::arg("h"), + py::arg("e_s") = nullptr, py::arg("e_t") = nullptr, py::arg("e_h") = nullptr) + .def("get_surface_pt", &Road::get_surface_pt, py::arg("s"), py::arg("t"), py::arg("vn") = nullptr) + .def("get_lane_border_line", py::overload_cast( + &Road::get_lane_border_line, py::const_), + py::arg("lane"), py::arg("s_start"), py::arg("s_end"), py::arg("eps"), py::arg("outer") = true) + .def("get_lane_border_line", py::overload_cast( + &Road::get_lane_border_line, py::const_), + py::arg("lane"), py::arg("eps"), py::arg("outer") = true) + .def("get_lane_mesh", py::overload_cast*>( + &Road::get_lane_mesh, py::const_), + py::arg("lane"), py::arg("s_start"), py::arg("s_end"), py::arg("eps"), py::arg("outline_indices") = nullptr) + .def("get_lane_mesh", py::overload_cast*>( + &Road::get_lane_mesh, py::const_), + py::arg("lane"), py::arg("eps"), py::arg("outline_indices") = nullptr) + .def("get_roadmark_mesh", &Road::get_roadmark_mesh) + .def("get_road_signal_mesh", &Road::get_road_signal_mesh) + .def("get_road_object_mesh", &Road::get_road_object_mesh) + .def("approximate_lane_border_linear", py::overload_cast( + &Road::approximate_lane_border_linear, py::const_), + py::arg("lane"), py::arg("s_start"), py::arg("s_end"), py::arg("eps"), py::arg("outer") = true) + .def("approximate_lane_border_linear", py::overload_cast( + &Road::approximate_lane_border_linear, py::const_), + py::arg("lane"), py::arg("eps"), py::arg("outer") = true) + .def_readwrite("length", &Road::length) + .def_readwrite("id", &Road::id) + .def_readwrite("junction", &Road::junction) + .def_readwrite("name", &Road::name) + .def_readwrite("left_hand_traffic", &Road::left_hand_traffic) + .def_readwrite("predecessor", &Road::predecessor) + .def_readwrite("successor", &Road::successor) + .def_readwrite("neighbors", &Road::neighbors) + .def_readwrite("lane_offset", &Road::lane_offset) + .def_readwrite("superelevation", &Road::superelevation) + .def_readwrite("crossfall", &Road::crossfall) + .def_readonly("ref_line", &Road::ref_line) + .def_readwrite("s_to_lanesection", &Road::s_to_lanesection) + .def_readwrite("s_to_type", &Road::s_to_type) + .def_readwrite("s_to_speed", &Road::s_to_speed) + .def_readwrite("id_to_object", &Road::id_to_object) + .def_readwrite("id_to_signal", &Road::id_to_signal); +} + +PYBIND11_MODULE(road, m) { + init_road(m); +} diff --git a/bindings/roadmark.cpp b/bindings/roadmark.cpp new file mode 100644 index 0000000..377306f --- /dev/null +++ b/bindings/roadmark.cpp @@ -0,0 +1,66 @@ +#include +#include +#include + +namespace py = pybind11; + +void init_roadmark(py::module_ &m) { + // Expose constants + m.attr("ROADMARK_WEIGHT_STANDARD_WIDTH") = odr::ROADMARK_WEIGHT_STANDARD_WIDTH; + m.attr("ROADMARK_WEIGHT_BOLD_WIDTH") = odr::ROADMARK_WEIGHT_BOLD_WIDTH; + + // Bind RoadMarksLine + py::class_(m, "RoadMarksLine") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("lane_id"), py::arg("group_s0"), + py::arg("width"), py::arg("length"), py::arg("space"), py::arg("t_offset"), py::arg("s_offset"), + py::arg("name"), py::arg("rule"), + "Constructs a RoadMarksLine with specified properties") + .def_readwrite("road_id", &odr::RoadMarksLine::road_id, "Road ID") + .def_readwrite("lanesection_s0", &odr::RoadMarksLine::lanesection_s0, "Lane section s0") + .def_readwrite("lane_id", &odr::RoadMarksLine::lane_id, "Lane ID") + .def_readwrite("group_s0", &odr::RoadMarksLine::group_s0, "Group start s-coordinate") + .def_readwrite("width", &odr::RoadMarksLine::width, "Width of the line") + .def_readwrite("length", &odr::RoadMarksLine::length, "Length of the line") + .def_readwrite("space", &odr::RoadMarksLine::space, "Spacing between lines") + .def_readwrite("t_offset", &odr::RoadMarksLine::t_offset, "Lateral offset") + .def_readwrite("s_offset", &odr::RoadMarksLine::s_offset, "Longitudinal offset") + .def_readwrite("name", &odr::RoadMarksLine::name, "Name of the line") + .def_readwrite("rule", &odr::RoadMarksLine::rule, "Rule (e.g., no passing)"); + + // Bind RoadMarkGroup + py::class_(m, "RoadMarkGroup") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("lane_id"), py::arg("width"), + py::arg("height"), py::arg("s_offset"), py::arg("type"), py::arg("weight"), + py::arg("color"), py::arg("material"), py::arg("lane_change"), + "Constructs a RoadMarkGroup with specified properties") + .def_readwrite("road_id", &odr::RoadMarkGroup::road_id, "Road ID") + .def_readwrite("lanesection_s0", &odr::RoadMarkGroup::lanesection_s0, "Lane section s0") + .def_readwrite("lane_id", &odr::RoadMarkGroup::lane_id, "Lane ID") + .def_readwrite("width", &odr::RoadMarkGroup::width, "Width of the group") + .def_readwrite("height", &odr::RoadMarkGroup::height, "Height of the group") + .def_readwrite("s_offset", &odr::RoadMarkGroup::s_offset, "Longitudinal offset") + .def_readwrite("type", &odr::RoadMarkGroup::type, "Type (e.g., solid, broken)") + .def_readwrite("weight", &odr::RoadMarkGroup::weight, "Weight (e.g., standard, bold)") + .def_readwrite("color", &odr::RoadMarkGroup::color, "Color of the group") + .def_readwrite("material", &odr::RoadMarkGroup::material, "Material of the group") + .def_readwrite("lane_change", &odr::RoadMarkGroup::lane_change, "Lane change rule") + .def_readwrite("roadmark_lines", &odr::RoadMarkGroup::roadmark_lines, "Set of RoadMarksLine objects"); + + // Bind RoadMark + py::class_(m, "RoadMark") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("lane_id"), py::arg("group_s0"), + py::arg("s_start"), py::arg("s_end"), py::arg("t_offset"), py::arg("width"), py::arg("type"), + "Constructs a RoadMark with specified properties") + .def_readwrite("road_id", &odr::RoadMark::road_id, "Road ID") + .def_readwrite("lanesection_s0", &odr::RoadMark::lanesection_s0, "Lane section s0") + .def_readwrite("lane_id", &odr::RoadMark::lane_id, "Lane ID") + .def_readwrite("group_s0", &odr::RoadMark::group_s0, "Group start s-coordinate") + .def_readwrite("s_start", &odr::RoadMark::s_start, "Start s-coordinate") + .def_readwrite("s_end", &odr::RoadMark::s_end, "End s-coordinate") + .def_readwrite("t_offset", &odr::RoadMark::t_offset, "Lateral offset") + .def_readwrite("width", &odr::RoadMark::width, "Width of the mark") + .def_readwrite("type", &odr::RoadMark::type, "Type (e.g., solid, broken)"); +} diff --git a/bindings/roadnetworkmesh.cpp b/bindings/roadnetworkmesh.cpp new file mode 100644 index 0000000..878e7a0 --- /dev/null +++ b/bindings/roadnetworkmesh.cpp @@ -0,0 +1,100 @@ +#include +#include +#include + +namespace py = pybind11; + +void init_roadnetworkmesh(py::module_ &m) { + // Bind RoadsMesh + py::class_(m, "RoadsMesh") + .def(py::init<>(), "Constructs an empty RoadsMesh") + .def("get_road_id", &odr::RoadsMesh::get_road_id, + py::arg("vert_idx"), + "Returns the road ID for the given vertex index") + .def("get_idx_interval_road", &odr::RoadsMesh::get_idx_interval_road, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the road at the given vertex index") + .def_readwrite("road_start_indices", &odr::RoadsMesh::road_start_indices, + "Map of vertex indices to road IDs") + .def_readwrite("vertices", &odr::Mesh3D::vertices, py::return_value_policy::reference, + "List of 3D vertices"); + + // Bind LanesMesh + py::class_(m, "LanesMesh") + .def(py::init<>(), "Constructs an empty LanesMesh") + .def("get_lanesec_s0", &odr::LanesMesh::get_lanesec_s0, + py::arg("vert_idx"), + "Returns the lane section s0 for the given vertex index") + .def("get_lane_id", &odr::LanesMesh::get_lane_id, + py::arg("vert_idx"), + "Returns the lane ID for the given vertex index") + .def("get_idx_interval_lanesec", &odr::LanesMesh::get_idx_interval_lanesec, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the lane section at the given vertex index") + .def("get_idx_interval_lane", &odr::LanesMesh::get_idx_interval_lane, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the lane at the given vertex index") + .def("get_lane_outline_indices", &odr::LanesMesh::get_lane_outline_indices, + py::return_value_policy::move, + "Returns the lane outline indices") + .def_readwrite("lanesec_start_indices", &odr::LanesMesh::lanesec_start_indices, + "Map of vertex indices to lane section s0 values") + .def_readwrite("lane_start_indices", &odr::LanesMesh::lane_start_indices, + "Map of vertex indices to lane IDs"); + + // Bind RoadmarksMesh + py::class_(m, "RoadmarksMesh") + .def(py::init<>(), "Constructs an empty RoadmarksMesh") + .def("get_roadmark_type", &odr::RoadmarksMesh::get_roadmark_type, + py::arg("vert_idx"), + "Returns the roadmark type for the given vertex index") + .def("get_idx_interval_roadmark", &odr::RoadmarksMesh::get_idx_interval_roadmark, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the roadmark at the given vertex index") + .def("get_roadmark_outline_indices", &odr::RoadmarksMesh::get_roadmark_outline_indices, + py::return_value_policy::move, + "Returns the roadmark outline indices") + .def_readwrite("roadmark_type_start_indices", &odr::RoadmarksMesh::roadmark_type_start_indices, + "Map of vertex indices to roadmark types"); + + // Bind RoadObjectsMesh + py::class_(m, "RoadObjectsMesh") + .def(py::init<>(), "Constructs an empty RoadObjectsMesh") + .def("get_road_object_id", &odr::RoadObjectsMesh::get_road_object_id, + py::arg("vert_idx"), + "Returns the road object ID for the given vertex index") + .def("get_idx_interval_road_object", &odr::RoadObjectsMesh::get_idx_interval_road_object, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the road object at the given vertex index") + .def_readwrite("road_object_start_indices", &odr::RoadObjectsMesh::road_object_start_indices, + "Map of vertex indices to road object IDs"); + + // Bind RoadSignalsMesh + py::class_(m, "RoadSignalsMesh") + .def(py::init<>(), "Constructs an empty RoadSignalsMesh") + .def("get_road_signal_id", &odr::RoadSignalsMesh::get_road_signal_id, + py::arg("vert_idx"), + "Returns the road signal ID for the given vertex index") + .def("get_idx_interval_signal", &odr::RoadSignalsMesh::get_idx_interval_signal, + py::arg("vert_idx"), + py::return_value_policy::move, + "Returns the index interval for the signal at the given vertex index") + .def_readwrite("road_signal_start_indices", &odr::RoadSignalsMesh::road_signal_start_indices, + "Map of vertex indices to road signal IDs"); + + // Bind RoadNetworkMesh + py::class_(m, "RoadNetworkMesh") + .def(py::init<>(), "Constructs an empty RoadNetworkMesh") + .def("get_mesh", &odr::RoadNetworkMesh::get_mesh, + py::return_value_policy::copy, + "Returns the combined Mesh3D") + .def_readwrite("lanes_mesh", &odr::RoadNetworkMesh::lanes_mesh, "Lanes mesh") + .def_readwrite("roadmarks_mesh", &odr::RoadNetworkMesh::roadmarks_mesh, "Roadmarks mesh") + .def_readwrite("road_objects_mesh", &odr::RoadNetworkMesh::road_objects_mesh, "Road objects mesh") + .def_readwrite("road_signals_mesh", &odr::RoadNetworkMesh::road_signals_mesh, "Road signals mesh"); +} diff --git a/bindings/roadobject.cpp b/bindings/roadobject.cpp new file mode 100644 index 0000000..8f93ab9 --- /dev/null +++ b/bindings/roadobject.cpp @@ -0,0 +1,97 @@ +#include +#include +#include +#include +#include +#include + +namespace py = pybind11; + +void init_roadobject(py::module_ &m) { + // Bind RoadObjectRepeat + py::class_(m, "RoadObjectRepeat") + .def(py::init(), + py::arg("s0"), py::arg("length"), py::arg("distance"), py::arg("t_start"), py::arg("t_end"), + py::arg("width_start"), py::arg("width_end"), py::arg("height_start"), py::arg("height_end"), + py::arg("z_offset_start"), py::arg("z_offset_end"), + "Constructs a RoadObjectRepeat with specified properties") + .def_readwrite("s0", &odr::RoadObjectRepeat::s0, "Start s-coordinate") + .def_readwrite("length", &odr::RoadObjectRepeat::length, "Length of repetition") + .def_readwrite("distance", &odr::RoadObjectRepeat::distance, "Distance between repetitions") + .def_readwrite("t_start", &odr::RoadObjectRepeat::t_start, "Start t-coordinate") + .def_readwrite("t_end", &odr::RoadObjectRepeat::t_end, "End t-coordinate") + .def_readwrite("width_start", &odr::RoadObjectRepeat::width_start, "Start width") + .def_readwrite("width_end", &odr::RoadObjectRepeat::width_end, "End width") + .def_readwrite("height_start", &odr::RoadObjectRepeat::height_start, "Start height") + .def_readwrite("height_end", &odr::RoadObjectRepeat::height_end, "End height") + .def_readwrite("z_offset_start", &odr::RoadObjectRepeat::z_offset_start, "Start z-offset") + .def_readwrite("z_offset_end", &odr::RoadObjectRepeat::z_offset_end, "End z-offset"); + + // Bind RoadObjectCorner::Type enum + py::enum_(m, "RoadObjectCornerType") + .value("Type_Local_RelZ", odr::RoadObjectCorner::Type::Type_Local_RelZ, "Z relative to road’s reference line") + .value("Type_Local_AbsZ", odr::RoadObjectCorner::Type::Type_Local_AbsZ, "Absolute z value") + .value("Type_Road", odr::RoadObjectCorner::Type::Type_Road, "Road-based") + .export_values(); + + // Bind RoadObjectCorner + py::class_(m, "RoadObjectCorner") + .def(py::init(), + py::arg("id"), py::arg("pt"), py::arg("height"), py::arg("type"), + "Constructs a RoadObjectCorner with ID, point, height, and type") + .def_readwrite("id", &odr::RoadObjectCorner::id, "Corner ID") + .def_readwrite("pt", &odr::RoadObjectCorner::pt, "3D point") + .def_readwrite("height", &odr::RoadObjectCorner::height, "Height") + .def_readwrite("type", &odr::RoadObjectCorner::type, "Corner type"); + + // Bind RoadObjectOutline + py::class_(m, "RoadObjectOutline") + .def(py::init(), + py::arg("id"), py::arg("fill_type"), py::arg("lane_type"), py::arg("outer"), py::arg("closed"), + "Constructs a RoadObjectOutline with specified properties") + .def_readwrite("id", &odr::RoadObjectOutline::id, "Outline ID") + .def_readwrite("fill_type", &odr::RoadObjectOutline::fill_type, "Fill type") + .def_readwrite("lane_type", &odr::RoadObjectOutline::lane_type, "Lane type") + .def_readwrite("outer", &odr::RoadObjectOutline::outer, "Outer flag") + .def_readwrite("closed", &odr::RoadObjectOutline::closed, "Closed flag") + .def_readwrite("outline", &odr::RoadObjectOutline::outline, "List of RoadObjectCorner objects"); + + // Bind RoadObject + py::class_(m, "RoadObject") + .def(py::init(), + py::arg("road_id"), py::arg("id"), py::arg("s0"), py::arg("t0"), py::arg("z0"), + py::arg("length"), py::arg("valid_length"), py::arg("width"), py::arg("radius"), + py::arg("height"), py::arg("hdg"), py::arg("pitch"), py::arg("roll"), + py::arg("type"), py::arg("name"), py::arg("orientation"), py::arg("subtype"), + py::arg("is_dynamic"), + "Constructs a RoadObject with specified properties") + .def_static("get_cylinder", &odr::RoadObject::get_cylinder, + py::arg("eps"), py::arg("radius"), py::arg("height"), + py::return_value_policy::move, + "Generates a cylinder Mesh3D") + .def_static("get_box", &odr::RoadObject::get_box, + py::arg("width"), py::arg("length"), py::arg("height"), + py::return_value_policy::move, + "Generates a box Mesh3D") + .def_readwrite("road_id", &odr::RoadObject::road_id, "Road ID") + .def_readwrite("id", &odr::RoadObject::id, "Object ID") + .def_readwrite("type", &odr::RoadObject::type, "Object type") + .def_readwrite("name", &odr::RoadObject::name, "Object name") + .def_readwrite("orientation", &odr::RoadObject::orientation, "Orientation") + .def_readwrite("subtype", &odr::RoadObject::subtype, "Subtype") + .def_readwrite("s0", &odr::RoadObject::s0, "Start s-coordinate") + .def_readwrite("t0", &odr::RoadObject::t0, "Start t-coordinate") + .def_readwrite("z0", &odr::RoadObject::z0, "Start z-coordinate") + .def_readwrite("length", &odr::RoadObject::length, "Length") + .def_readwrite("valid_length", &odr::RoadObject::valid_length, "Valid length") + .def_readwrite("width", &odr::RoadObject::width, "Width") + .def_readwrite("radius", &odr::RoadObject::radius, "Radius") + .def_readwrite("height", &odr::RoadObject::height, "Height") + .def_readwrite("hdg", &odr::RoadObject::hdg, "Heading") + .def_readwrite("pitch", &odr::RoadObject::pitch, "Pitch") + .def_readwrite("roll", &odr::RoadObject::roll, "Roll") + .def_readwrite("is_dynamic", &odr::RoadObject::is_dynamic, "Dynamic flag") + .def_readwrite("repeats", &odr::RoadObject::repeats, "List of RoadObjectRepeat objects") + .def_readwrite("outlines", &odr::RoadObject::outlines, "List of RoadObjectOutline objects") + .def_readwrite("lane_validities", &odr::RoadObject::lane_validities, "List of LaneValidityRecord objects"); +} diff --git a/bindings/roadsignal.cpp b/bindings/roadsignal.cpp new file mode 100644 index 0000000..8a2a7fe --- /dev/null +++ b/bindings/roadsignal.cpp @@ -0,0 +1,43 @@ +#include +#include +#include +#include +#include + +namespace py = pybind11; + +void init_roadsignal(py::module_ &m) { + // Bind RoadSignal + py::class_(m, "RoadSignal") + .def(py::init(), + py::arg("road_id"), py::arg("id"), py::arg("name"), py::arg("s0"), py::arg("t0"), + py::arg("is_dynamic"), py::arg("zOffset"), py::arg("value"), py::arg("height"), + py::arg("width"), py::arg("hOffset"), py::arg("pitch"), py::arg("roll"), + py::arg("orientation"), py::arg("country"), py::arg("type"), py::arg("subtype"), + py::arg("unit"), py::arg("text"), + "Constructs a RoadSignal with specified properties") + .def_static("get_box", &odr::RoadSignal::get_box, + py::arg("width"), py::arg("length"), py::arg("height"), + py::return_value_policy::move, + "Generates a box Mesh3D") + .def_readwrite("road_id", &odr::RoadSignal::road_id, "Road ID") + .def_readwrite("id", &odr::RoadSignal::id, "Signal ID") + .def_readwrite("name", &odr::RoadSignal::name, "Signal name") + .def_readwrite("s0", &odr::RoadSignal::s0, "Start s-coordinate") + .def_readwrite("t0", &odr::RoadSignal::t0, "Start t-coordinate") + .def_readwrite("is_dynamic", &odr::RoadSignal::is_dynamic, "Dynamic flag") + .def_readwrite("zOffset", &odr::RoadSignal::zOffset, "Z offset") + .def_readwrite("value", &odr::RoadSignal::value, "Signal value") + .def_readwrite("height", &odr::RoadSignal::height, "Height") + .def_readwrite("width", &odr::RoadSignal::width, "Width") + .def_readwrite("hOffset", &odr::RoadSignal::hOffset, "Heading offset") + .def_readwrite("pitch", &odr::RoadSignal::pitch, "Pitch") + .def_readwrite("roll", &odr::RoadSignal::roll, "Roll") + .def_readwrite("orientation", &odr::RoadSignal::orientation, "Orientation") + .def_readwrite("country", &odr::RoadSignal::country, "Country code") + .def_readwrite("type", &odr::RoadSignal::type, "Signal type") + .def_readwrite("subtype", &odr::RoadSignal::subtype, "Signal subtype") + .def_readwrite("unit", &odr::RoadSignal::unit, "Unit of value") + .def_readwrite("text", &odr::RoadSignal::text, "Signal text") + .def_readwrite("lane_validities", &odr::RoadSignal::lane_validities, "List of LaneValidityRecord objects"); +} diff --git a/bindings/routinggraph.cpp b/bindings/routinggraph.cpp new file mode 100644 index 0000000..e013e11 --- /dev/null +++ b/bindings/routinggraph.cpp @@ -0,0 +1,56 @@ +#include +#include +#include + +namespace py = pybind11; + +void init_routinggraph(py::module_ &m) { + // Bind RoutingGraphEdge + py::class_(m, "RoutingGraphEdge") + .def(py::init(), + py::arg("from"), py::arg("to"), py::arg("length"), + "Constructs a RoutingGraphEdge with from and to LaneKeys and length") + .def_readwrite("from", &odr::RoutingGraphEdge::from, "Source LaneKey") + .def_readwrite("to", &odr::RoutingGraphEdge::to, "Destination LaneKey") + .def_readwrite("weight", &odr::RoutingGraphEdge::weight, "Edge weight") + .def(" __eq__", [](const odr::RoutingGraphEdge& a, const odr::RoutingGraphEdge& b) { + return a.from == b.from && a.to == b.to && a.weight == b.weight; + }, "Equality comparison"); + + // Bind WeightedLaneKey + py::class_(m, "WeightedLaneKey") + .def(py::init(), + py::arg("lane_key"), py::arg("weight"), + "Constructs a WeightedLaneKey from a LaneKey and weight") + .def(py::init(), + py::arg("road_id"), py::arg("lanesection_s0"), py::arg("lane_id"), py::arg("weight"), + "Constructs a WeightedLaneKey with road ID, lane section s0, lane ID, and weight") + .def_readwrite("weight", &odr::WeightedLaneKey::weight, "Weight of the lane key") + .def(" __eq__", [](const odr::WeightedLaneKey& a, const odr::WeightedLaneKey& b) { + return static_cast(a) == static_cast(b) && a.weight == b.weight; + }, "Equality comparison"); + + // Bind RoutingGraph + py::class_(m, "RoutingGraph") + .def(py::init<>(), "Constructs an empty RoutingGraph") + .def("add_edge", &odr::RoutingGraph::add_edge, + py::arg("edge"), + "Adds an edge to the graph") + .def("get_lane_successors", &odr::RoutingGraph::get_lane_successors, + py::arg("lane_key"), + py::return_value_policy::move, + "Returns the successor LaneKeys for the given LaneKey") + .def("get_lane_predecessors", &odr::RoutingGraph::get_lane_predecessors, + py::arg("lane_key"), + py::return_value_policy::move, + "Returns the predecessor LaneKeys for the given LaneKey") + .def("shortest_path", &odr::RoutingGraph::shortest_path, + py::arg("from"), py::arg("to"), + py::return_value_policy::move, + "Computes the shortest path between two LaneKeys") + .def_readwrite("edges", &odr::RoutingGraph::edges, "Set of RoutingGraphEdges") + .def_readwrite("lane_key_to_successors", &odr::RoutingGraph::lane_key_to_successors, + "Map of LaneKeys to their successor WeightedLaneKeys") + .def_readwrite("lane_key_to_predecessors", &odr::RoutingGraph::lane_key_to_predecessors, + "Map of LaneKeys to their predecessor WeightedLaneKeys"); +} diff --git a/bindings/xml_node.cpp b/bindings/xml_node.cpp new file mode 100644 index 0000000..9e57850 --- /dev/null +++ b/bindings/xml_node.cpp @@ -0,0 +1,11 @@ +#include +#include "XmlNode.h" + +namespace py = pybind11; +using namespace odr; + +void init_xml_node(py::module_ &m) +{ + //py::class_(m, "XmlNode"); + py::class_>(m, "XmlNode"); +} diff --git a/pybind11 b/pybind11 new file mode 160000 index 0000000..c630e22 --- /dev/null +++ b/pybind11 @@ -0,0 +1 @@ +Subproject commit c630e22c1cb90251ac5316e508dde38545a4bdd2 diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 0000000..7f2f68d --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,16 @@ +[project] +name = "opendrive" +version = "0.0.1" +description = "Python bindings for libOpenDRIVE" +readme = "README.md" +license = {text = "Apache-2.0"} +authors = [ + {name = "Christian Contreras", email = "chrisjcc.physics@gmail.com"} +] +urls = { "Homepage" = "https://github.com/pageldev/libOpenDRIVE" } +requires-python = ">=3.11" +dependencies = [] + +[build-system] +requires = ["setuptools>=64", "pybind11>=2.9"] +build-backend = "setuptools.build_meta" diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..5fc38a3 --- /dev/null +++ b/setup.py @@ -0,0 +1,42 @@ +from setuptools import setup +from pybind11.setup_helpers import Pybind11Extension +import os + +# Path to the build directory where e.g. libOpenDrive.dylib is located +build_dir = os.path.abspath("build") + +ext_modules = [ + Pybind11Extension( + "opendrive", + [ + "bindings/bindings.cpp", + "bindings/opendrivemap.cpp", + "bindings/junction.cpp", + "bindings/lane.cpp", + "bindings/lanesection.cpp", + "bindings/road.cpp", + "bindings/refline.cpp", + "bindings/mesh.cpp", + "bindings/math.cpp", + "bindings/routinggraph.cpp", + "bindings/roadmark.cpp", + "bindings/roadnetworkmesh.cpp", + "bindings/roadobject.cpp", + "bindings/lanevalidityrecord.cpp", + "bindings/roadsignal.cpp", + "bindings/xml_node.cpp", + ], + include_dirs=[ + "include", # libOpenDRIVE headers + os.path.join(build_dir, "_deps/pugixml-src/src"), # pugixml headers + ], + define_macros=[("NDEBUG", None)], + library_dirs=[build_dir], # Where libOpenDrive.dylib is located + libraries=["OpenDrive"], # Correct library name (without 'lib' prefix) + extra_link_args=["-Wl,-rpath," + build_dir], # Add rpath for macOS + ), +] + +setup( + ext_modules=ext_modules, +) diff --git a/tests/test_binding.py b/tests/test_binding.py new file mode 100644 index 0000000..08534b5 --- /dev/null +++ b/tests/test_binding.py @@ -0,0 +1,122 @@ +import pytest +import os +from opendrive import OpenDriveMap, RoadNetworkMesh, LanesMesh, RoadmarksMesh, RoadObjectsMesh, RoadSignalsMesh, RoutingGraph, Mesh3D + +# Fixture to load the OpenDriveMap +@pytest.fixture +def odr_map(): + file_path = "tests/test.xodr" + if not os.path.exists(file_path): + pytest.skip(f"File '{file_path}' not found in {os.getcwd()}") + try: + map_instance = OpenDriveMap(file_path, center_map=False) + return map_instance + except Exception as e: + pytest.fail(f"Failed to load OpenDriveMap: {e}") + +# Fixture to get the road network mesh +@pytest.fixture +def road_network_mesh(odr_map): + eps = 0.1 + try: + return odr_map.get_road_network_mesh(eps) + except Exception as e: + pytest.fail(f"Failed to get road network mesh: {e}") + +def test_opendrive_map_attributes(odr_map): + """Test the attributes of OpenDriveMap.""" + assert isinstance(odr_map.xodr_file, str), "xodr_file is not a string" + assert isinstance(odr_map.proj4, str), "proj4 is not a string" + assert isinstance(odr_map.x_offs, float), "x_offs is not a float" + assert isinstance(odr_map.y_offs, float), "y_offs is not a float" + +def test_roads(odr_map): + """Test the roads retrieved from OpenDriveMap.""" + roads = odr_map.get_roads() + assert isinstance(roads, list), "Roads is not a list" + if roads: + assert isinstance(odr_map.id_to_road, dict), "id_to_road is not a dict" + print(f"Found {len(roads)} roads") + +def test_junctions(odr_map): + """Test the junctions retrieved from OpenDriveMap.""" + junctions = odr_map.get_junctions() + assert isinstance(junctions, list), "Junctions is not a list" + if junctions: + assert isinstance(odr_map.id_to_junction, dict), "id_to_junction is not a dict" + print(f"Found {len(junctions)} junctions") + +def test_road_network_mesh(road_network_mesh): + """Test the road network mesh.""" + assert isinstance(road_network_mesh, RoadNetworkMesh), "road_network_mesh is not a RoadNetworkMesh" + assert isinstance(road_network_mesh.get_mesh(), Mesh3D), "get_mesh() does not return a Mesh3D" + +def test_lanes_mesh(road_network_mesh): + """Test the lanes mesh and its attributes.""" + lanes_mesh = road_network_mesh.lanes_mesh + assert isinstance(lanes_mesh, LanesMesh), "lanes_mesh is not a LanesMesh" + + print(f"LanesMesh vertices exist: {hasattr(lanes_mesh, 'vertices')}") + print(f"LanesMesh vertices length: {len(lanes_mesh.vertices) if hasattr(lanes_mesh, 'vertices') else 'N/A'}") + if not hasattr(lanes_mesh, "vertices") or not lanes_mesh.vertices: + pytest.skip("No vertices in lanes_mesh") + + vert_idx = 0 + assert isinstance(lanes_mesh.get_lanesec_s0(vert_idx), (float, int)), "get_lanesec_s0 does not return a number" + assert isinstance(lanes_mesh.get_lane_id(vert_idx), (str, int)), "get_lane_id does not return a string or int" + assert isinstance(lanes_mesh.get_idx_interval_lanesec(vert_idx), (list, tuple)), "get_idx_interval_lanesec does not return a list or tuple" + assert isinstance(lanes_mesh.get_idx_interval_lane(vert_idx), (list, tuple)), "get_idx_interval_lane does not return a list or tuple" + assert isinstance(lanes_mesh.get_lane_outline_indices(), (list, tuple)), "get_lane_outline_indices does not return a list or tuple" + assert isinstance(lanes_mesh.lanesec_start_indices, dict), "lanesec_start_indices is not a dict" + assert isinstance(lanes_mesh.lane_start_indices, dict), "lane_start_indices is not a dict" + +def test_roadmarks_mesh(road_network_mesh): + """Test the roadmarks mesh and its attributes.""" + roadmarks_mesh = road_network_mesh.roadmarks_mesh + assert isinstance(roadmarks_mesh, RoadmarksMesh), "roadmarks_mesh is not a RoadmarksMesh" + + print(f"RoadmarksMesh vertices exist: {hasattr(roadmarks_mesh, 'vertices')}") + print(f"RoadmarksMesh vertices length: {len(roadmarks_mesh.vertices) if hasattr(roadmarks_mesh, 'vertices') else 'N/A'}") + if not hasattr(roadmarks_mesh, "vertices") or not roadmarks_mesh.vertices: + pytest.skip("No vertices in roadmarks_mesh") + + vert_idx = 0 + assert isinstance(roadmarks_mesh.get_roadmark_type(vert_idx), str), "get_roadmark_type does not return a string" + assert isinstance(roadmarks_mesh.get_idx_interval_roadmark(vert_idx), (list, tuple)), "get_idx_interval_roadmark does not return a list or tuple" + assert isinstance(roadmarks_mesh.get_roadmark_outline_indices(), (list, tuple)), "get_roadmark_outline_indices does not return a list or tuple" + assert isinstance(roadmarks_mesh.roadmark_type_start_indices, dict), "roadmark_type_start_indices is not a dict" + +def test_road_objects_mesh(road_network_mesh): + """Test the road objects mesh and its attributes.""" + road_objects_mesh = road_network_mesh.road_objects_mesh + assert isinstance(road_objects_mesh, RoadObjectsMesh), "road_objects_mesh is not a RoadObjectsMesh" + + print(f"RoadObjectsMesh vertices exist: {hasattr(road_objects_mesh, 'vertices')}") + print(f"RoadObjectsMesh vertices length: {len(road_objects_mesh.vertices) if hasattr(road_objects_mesh, 'vertices') else 'N/A'}") + if not hasattr(road_objects_mesh, "vertices") or not road_objects_mesh.vertices: + pytest.skip("No vertices in road_objects_mesh") + + vert_idx = 0 + assert isinstance(road_objects_mesh.get_road_object_id(vert_idx), (str, int)), "get_road_object_id does not return a string or int" + assert isinstance(road_objects_mesh.get_idx_interval_road_object(vert_idx), (list, tuple)), "get_idx_interval_road_object does not return a list or tuple" + assert isinstance(road_objects_mesh.road_object_start_indices, dict), "road_object_start_indices is not a dict" + +def test_road_signals_mesh(road_network_mesh): + """Test the road signals mesh and its attributes.""" + road_signals_mesh = road_network_mesh.road_signals_mesh + assert isinstance(road_signals_mesh, RoadSignalsMesh), "road_signals_mesh is not a RoadSignalsMesh" + + print(f"RoadSignalsMesh vertices exist: {hasattr(road_signals_mesh, 'vertices')}") + print(f"RoadSignalsMesh vertices length: {len(road_signals_mesh.vertices) if hasattr(road_signals_mesh, 'vertices') else 'N/A'}") + if not hasattr(road_signals_mesh, "vertices") or not road_signals_mesh.vertices: + pytest.skip("No vertices in road_signals_mesh") + + vert_idx = 0 + assert isinstance(road_signals_mesh.get_road_signal_id(vert_idx), (str, int)), "get_road_signal_id does not return a string or int" + assert isinstance(road_signals_mesh.get_idx_interval_signal(vert_idx), (list, tuple)), "get_idx_interval_signal does not return a list or tuple" + assert isinstance(road_signals_mesh.road_signal_start_indices, dict), "road_signal_start_indices is not a dict" + +def test_routing_graph(odr_map): + """Test the routing graph.""" + routing_graph = odr_map.get_routing_graph() + assert isinstance(routing_graph, RoutingGraph), "routing_graph is not a RoutingGraph" diff --git a/tests/test_opendrivemap.py b/tests/test_opendrivemap.py new file mode 100644 index 0000000..04e55c9 --- /dev/null +++ b/tests/test_opendrivemap.py @@ -0,0 +1,26 @@ +import inspect + +import pytest +from opendrive import OpenDriveMap, LaneKey + +def test_basic_opendrivemap_check(): + odr_map = OpenDriveMap("tests/test.xodr", center_map=False) + assert odr_map.xml_parse_result + assert len(odr_map.get_roads()) > 0 + + # test routing + graph = odr_map.get_routing_graph() + path = graph.shortest_path(LaneKey("43", 0.0, 1), LaneKey("41", 0.0, 1)) + assert len(path) == 15 + + for road in odr_map.get_roads(): + print(f"road: {road.id}, length: {road.length}") + assert road.length >= 0.0 + assert len(road.s_to_lanesection) > 0 + for lanesection in road.get_lanesections(): + s_start = lanesection.s0 + s_end = road.get_lanesection_end(lanesection) + assert s_start >= 0.0 + assert s_end > s_start + for lane in lanesection.get_lanes(): + roadmarks = lane.get_roadmarks(s_start, s_end)