From 7f39d23ea7ad98bb85925f986e1b87b0084a3c65 Mon Sep 17 00:00:00 2001 From: Oliver Boettcher Date: Mon, 6 Feb 2023 10:54:23 +0100 Subject: [PATCH 1/4] added simple support und small implementation for Road Signals --- CMakeLists.txt | 1 + include/OpenDriveMap.h | 1 + include/Road.h | 4 +++ include/RoadSignal.h | 64 ++++++++++++++++++++++++++++++++++++++++++ src/OpenDriveMap.cpp | 38 +++++++++++++++++++++++++ src/Road.cpp | 26 ++++++++++++++++- src/RoadSignal.cpp | 46 ++++++++++++++++++++++++++++++ 7 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 include/RoadSignal.h create mode 100644 src/RoadSignal.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e51903..0c8c0d1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -37,6 +37,7 @@ add_library(OpenDrive SHARED src/RefLine.cpp src/Road.cpp src/RoadMark.cpp + src/RoadSignal.cpp src/RoadObject.cpp src/RoutingGraph.cpp Thirdparty/pugixml/pugixml.cpp diff --git a/include/OpenDriveMap.h b/include/OpenDriveMap.h index 847b272..e97b9af 100644 --- a/include/OpenDriveMap.h +++ b/include/OpenDriveMap.h @@ -17,6 +17,7 @@ struct OpenDriveMapConfig bool with_lateralProfile = true; bool with_laneHeight = true; bool with_road_objects = true; + bool with_road_signals = true; bool center_map = true; bool abs_z_for_for_local_road_obj_outline = false; }; diff --git a/include/Road.h b/include/Road.h index e4e65fb..afed703 100644 --- a/include/Road.h +++ b/include/Road.h @@ -4,6 +4,7 @@ #include "Math.hpp" #include "Mesh.h" #include "RefLine.h" +#include "RoadSignal.h" #include "RoadObject.h" #include "XmlNode.h" @@ -81,6 +82,7 @@ class Road : public XmlNode Road(std::string id, double length, std::string junction, std::string name); std::vector get_lanesections() const; + std::vector get_road_signals() const; std::vector get_road_objects() const; double get_lanesection_s0(const double s) const; @@ -102,6 +104,7 @@ class Road : public XmlNode Mesh3D get_lane_mesh(const Lane& lane, const double eps, std::vector* outline_indices = nullptr) const; Mesh3D get_roadmark_mesh(const Lane& lane, const RoadMark& roadmark, const double eps) const; + Mesh3D get_road_signal_mesh(const RoadSignal& road_signal) const; Mesh3D get_road_object_mesh(const RoadObject& road_object, const double eps) const; std::set @@ -126,6 +129,7 @@ class Road : public XmlNode std::map s_to_type; std::map s_to_speed; std::map id_to_object; + std::map id_to_signal; }; } // namespace odr diff --git a/include/RoadSignal.h b/include/RoadSignal.h new file mode 100644 index 0000000..6c46982 --- /dev/null +++ b/include/RoadSignal.h @@ -0,0 +1,64 @@ +#pragma once +#include "Math.hpp" +#include "Mesh.h" +#include "XmlNode.h" + +#include +#include + +namespace odr +{ + struct RoadSignal : public XmlNode + { + RoadSignal(std::string road_id, + std::string id, + double s0, + double t0, + double z0, + double width, + double height, + double roll, + double pitch, + double yaw, + double h_offset, + double value, + std::string country, + std::string country_revision, + std::string type, + std::string subtype, + std::string text, + std::string unit, + std::string name, + bool dynamic + ); + + static Mesh3D get_box(const double width, + const double length, + const double height); + + std::string road_id = ""; + std::string id = ""; + std::string name = ""; + std::string country = ""; + std::string country_revision = ""; + std::string type = ""; + std::string subtype = ""; + std::string text = ""; + std::string unit = ""; + + double s0 = 0.0; + double t0 = 0.0; + double z0 = 0.0; + double width = 0.0; + double height = 0.0; + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; + double hdg = 0.0; + + double value = 0.0; + + bool dynamic = false; + }; + +} \ No newline at end of file diff --git a/src/OpenDriveMap.cpp b/src/OpenDriveMap.cpp index b5219a4..be2e753 100644 --- a/src/OpenDriveMap.cpp +++ b/src/OpenDriveMap.cpp @@ -542,6 +542,44 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file, const OpenDriveMapConfi } } } + if (config.with_road_signals) + { + for (pugi::xml_node signal_node : road_node.child("signals").children("signal")) + { + std::string road_signal_id = signal_node.attribute("id").as_string(""); + CHECK_AND_REPAIR(road.id_to_signal.find(road_signal_id) == road.id_to_signal.end(), + (std::string("object::id already exists - ") + road_signal_id).c_str(), + road_signal_id = road_signal_id + std::string("_dup")); + + RoadSignal& signal_object = road.id_to_signal + .insert({road_signal_id, + RoadSignal(road_id, + road_signal_id, + signal_node.attribute("s").as_double(0), + signal_node.attribute("t").as_double(0), + signal_node.attribute("zOffset").as_double(0), + signal_node.attribute("width").as_double(0), + signal_node.attribute("height").as_double(0), + signal_node.attribute("roll").as_double(0), + signal_node.attribute("pitch").as_double(0), + signal_node.attribute("yaw").as_double(0), + signal_node.attribute("hOffset").as_double(0), + signal_node.attribute("value").as_double(0), + signal_node.attribute("country").as_string(""), + signal_node.attribute("countryRevision").as_string(""), + signal_node.attribute("type").as_string(""), + signal_node.attribute("subtype").as_string(""), + signal_node.attribute("text").as_string(""), + signal_node.attribute("unit").as_string(""), + signal_node.attribute("name").as_string(""), + signal_node.attribute("dynamic").as_bool())}) + .first->second; + signal_object.xml_node = signal_node; + + CHECK_AND_REPAIR(signal_object.s0 >= 0, "signal::s < 0", signal_object.s0 = 0); + + } + } } } diff --git a/src/Road.cpp b/src/Road.cpp index 5aa970c..4bc1f61 100644 --- a/src/Road.cpp +++ b/src/Road.cpp @@ -50,7 +50,7 @@ RoadNeighbor::RoadNeighbor(std::string id, std::string side, std::string directi SpeedRecord::SpeedRecord(std::string max, std::string unit) : max(max), unit(unit) {} std::vector Road::get_lanesections() const { return get_map_values(this->s_to_lanesection); } - +std::vector Road::get_road_signals() const { return get_map_values(this->id_to_signal); } std::vector Road::get_road_objects() const { return get_map_values(this->id_to_object); } Road::Road(std::string id, double length, std::string junction, std::string name) : @@ -331,6 +331,30 @@ Mesh3D Road::get_roadmark_mesh(const Lane& lane, const RoadMark& roadmark, const return out_mesh; } +Mesh3D Road::get_road_signal_mesh(const RoadSignal& road_signal) const +{ + const Mat3D rot_mat = EulerAnglesToMatrix(road_signal.roll, road_signal.pitch, road_signal.hdg); + const double s = road_signal.s0; + const double t = road_signal.t0; + const double z = road_signal.z0; + const double height = road_signal.height; + const double width = road_signal.width; + Mesh3D road_signal_mesh; + road_signal_mesh = road_signal.get_box(width, 0.2, height); + Vec3D e_s, e_t, e_h; + const Vec3D p0 = this->get_xyz(s, t, z, &e_s, &e_t, &e_h); + const Mat3D base_mat{{{e_s[0], e_t[0], e_h[0]}, {e_s[1], e_t[1], e_h[1]}, {e_s[2], e_t[2], e_h[2]}}}; + for (Vec3D& pt_uvz : road_signal_mesh.vertices) + { + pt_uvz = MatVecMultiplication(rot_mat, pt_uvz); + pt_uvz = MatVecMultiplication(base_mat, pt_uvz); + pt_uvz = add(pt_uvz, p0); + + road_signal_mesh.st_coordinates.push_back({s, t}); + } + return road_signal_mesh; +} + Mesh3D Road::get_road_object_mesh(const RoadObject& road_object, const double eps) const { std::vector repeats_copy = road_object.repeats; // make copy to keep method const diff --git a/src/RoadSignal.cpp b/src/RoadSignal.cpp new file mode 100644 index 0000000..1aad963 --- /dev/null +++ b/src/RoadSignal.cpp @@ -0,0 +1,46 @@ +#include "RoadSignal.h" + +namespace odr { + + RoadSignal::RoadSignal(std::string road_id, + std::string id, + double s0, + double t0, + double z0, + double width, + double height, + double roll, + double pitch, + double yaw, + double h_offset, + double value, + std::string country, + std::string country_revision, + std::string type, + std::string subtype, + std::string text, + std::string unit, + std::string name, + bool dynamic): + road_id(road_id), + id(id), s0(s0), t0(t0),z0(z0),width(width), height(height), roll(roll), pitch(pitch),yaw(yaw), + hdg(h_offset),name(name) ,value(value), country(country), country_revision(country_revision), + type(type), subtype(subtype), text(text), unit(unit),dynamic(dynamic) + {} + Mesh3D RoadSignal::get_box(const double w, const double l, const double h) + { + Mesh3D box_mesh; + box_mesh.vertices = {Vec3D{l / 2, w / 2, 0}, + Vec3D{-l / 2, w / 2, 0}, + Vec3D{-l / 2, -w / 2, 0}, + Vec3D{l / 2, -w / 2, 0}, + Vec3D{l / 2, w / 2, h}, + Vec3D{-l / 2, w / 2, h}, + Vec3D{-l / 2, -w / 2, h}, + Vec3D{l / 2, -w / 2, h}}; + box_mesh.indices = {0, 3, 1, 3, 2, 1, 4, 5, 7, 7, 5, 6, 7, 6, 3, 3, 6, 2, 5, 4, 1, 1, 4, 0, 0, 4, 7, 7, 3, 0, 1, 6, 5, 1, 2, 6}; + + return box_mesh; + } + +}// namespace odr \ No newline at end of file From 01f0fe371b071887011dd9ca4947eed1beac8d22 Mon Sep 17 00:00:00 2001 From: Michael Scholz Date: Tue, 5 Sep 2023 12:32:36 +0200 Subject: [PATCH 2/4] Add SignalsMesh to RoadNetworkMesh and OpenDriveMap --- include/RoadNetworkMesh.h | 9 +++++++++ src/OpenDriveMap.cpp | 9 +++++++++ src/RoadNetworkMesh.cpp | 11 +++++++++++ 3 files changed, 29 insertions(+) diff --git a/include/RoadNetworkMesh.h b/include/RoadNetworkMesh.h index 9ec4a30..f3b9c66 100644 --- a/include/RoadNetworkMesh.h +++ b/include/RoadNetworkMesh.h @@ -56,6 +56,14 @@ struct RoadObjectsMesh : public RoadsMesh std::map road_object_start_indices; }; +struct SignalsMesh : public RoadsMesh +{ + std::string get_signal_id(const std::size_t vert_idx) const; + std::array get_idx_interval_signal(const std::size_t vert_idx) const; + + std::map signal_start_indices; +}; + struct RoadNetworkMesh { Mesh3D get_mesh() const; @@ -63,6 +71,7 @@ struct RoadNetworkMesh LanesMesh lanes_mesh; RoadmarksMesh roadmarks_mesh; RoadObjectsMesh road_objects_mesh; + SignalsMesh signals_mesh; }; } // namespace odr \ No newline at end of file diff --git a/src/OpenDriveMap.cpp b/src/OpenDriveMap.cpp index 5615157..c160bb4 100644 --- a/src/OpenDriveMap.cpp +++ b/src/OpenDriveMap.cpp @@ -685,6 +685,7 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const LanesMesh& lanes_mesh = out_mesh.lanes_mesh; RoadmarksMesh& roadmarks_mesh = out_mesh.roadmarks_mesh; RoadObjectsMesh& road_objects_mesh = out_mesh.road_objects_mesh; + SignalsMesh& signals_mesh = out_mesh.signals_mesh; for (const auto& id_road : this->id_to_road) { @@ -724,6 +725,14 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const road_objects_mesh.road_object_start_indices[road_objs_idx_offset] = road_object.id; road_objects_mesh.add_mesh(road.get_road_object_mesh(road_object, eps)); } + + for (const auto& id_signal : road.id_to_signal) + { + const Signal& signal = id_signal.second; + const std::size_t signals_idx_offset = signals_mesh.vertices.size(); + signals_mesh.signal_start_indices[signals_idx_offset] = signal.id; + signals_mesh.add_mesh(road.get_signal_mesh(signal)); + } } return out_mesh; diff --git a/src/RoadNetworkMesh.cpp b/src/RoadNetworkMesh.cpp index 95190cf..9b9b221 100644 --- a/src/RoadNetworkMesh.cpp +++ b/src/RoadNetworkMesh.cpp @@ -84,12 +84,23 @@ std::array RoadObjectsMesh::get_idx_interval_road_object(const std::s return get_key_interval(this->road_object_start_indices, vert_idx, this->vertices.size()); } +std::string SignalsMesh::get_signal_id(const std::size_t vert_idx) const +{ + return get_nearest_lower_val(this->signal_start_indices, vert_idx); +} + +std::array SignalsMesh::get_idx_interval_signal(const std::size_t vert_idx) const +{ + return get_key_interval(this->signal_start_indices, vert_idx, this->vertices.size()); +} + Mesh3D RoadNetworkMesh::get_mesh() const { Mesh3D out_mesh; out_mesh.add_mesh(this->lanes_mesh); out_mesh.add_mesh(this->roadmarks_mesh); out_mesh.add_mesh(this->road_objects_mesh); + out_mesh.add_mesh(this->signals_mesh); return out_mesh; } From 6b0703626c897d22bd3a74cc8f9f7043051b38cc Mon Sep 17 00:00:00 2001 From: Michael Scholz Date: Tue, 5 Sep 2023 12:36:01 +0200 Subject: [PATCH 3/4] Parse road object and signal meshes in test.cpp --- test.cpp | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) diff --git a/test.cpp b/test.cpp index 16eeae3..81def5b 100644 --- a/test.cpp +++ b/test.cpp @@ -22,6 +22,8 @@ int main(int argc, char** argv) std::vector lane_pts; std::vector roadmark_pts; + std::vector road_object_pts; + std::vector signal_pts; for (odr::Road road : odr_map.get_roads()) { @@ -44,9 +46,22 @@ int main(int argc, char** argv) } } } + + for (odr::RoadObject road_object: road.get_road_objects()) + { + auto road_object_mesh = road.get_road_object_mesh(road_object, eps); + road_object_pts.insert(road_object_pts.end(), road_object_mesh.vertices.begin(), road_object_mesh.vertices.end()); + } + + for (odr::Signal signal: road.get_signals()) + { + auto signal_mesh = road.get_signal_mesh(signal); + signal_pts.insert(signal_pts.end(), signal_mesh.vertices.begin(), signal_mesh.vertices.end()); + } } - printf("Finished, got %lu lane points, %lu roadmark points\n", lane_pts.size(), roadmark_pts.size()); + printf("Finished, got %llu lane points, %llu roadmark points, %llu road object points, %llu signal points\n", + lane_pts.size(), roadmark_pts.size(), road_object_pts.size(), signal_pts.size()); odr::RoadNetworkMesh road_network_mesh = odr_map.get_road_network_mesh(eps); printf("Got road network mesh\n"); From 11fcb2143a329f2cb9cb88fd052f47da51b21d69 Mon Sep 17 00:00:00 2001 From: Michael Scholz Date: Tue, 12 Sep 2023 17:07:03 +0200 Subject: [PATCH 4/4] Rename Signal to RoadSignal --- CMakeLists.txt | 2 +- include/OpenDriveMap.h | 2 +- include/Road.h | 8 +-- include/RoadNetworkMesh.h | 8 +-- include/{Signal.h => RoadSignal.h} | 40 +++++++------- src/OpenDriveMap.cpp | 84 +++++++++++++++--------------- src/Road.cpp | 26 ++++----- src/RoadNetworkMesh.cpp | 10 ++-- src/{Signal.cpp => RoadSignal.cpp} | 42 +++++++-------- test.cpp | 12 ++--- 10 files changed, 117 insertions(+), 117 deletions(-) rename include/{Signal.h => RoadSignal.h} (54%) rename src/{Signal.cpp => RoadSignal.cpp} (50%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3b08be9..aaeeb59 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -36,7 +36,7 @@ add_library(OpenDrive SHARED src/RoadMark.cpp src/RoadNetworkMesh.cpp src/RoadObject.cpp - src/Signal.cpp + src/RoadSignal.cpp src/RoutingGraph.cpp thirdparty/pugixml/pugixml.cpp ) diff --git a/include/OpenDriveMap.h b/include/OpenDriveMap.h index ac48187..87d88d2 100644 --- a/include/OpenDriveMap.h +++ b/include/OpenDriveMap.h @@ -23,7 +23,7 @@ class OpenDriveMap const bool with_lane_height = true, const bool abs_z_for_for_local_road_obj_outline = false, const bool fix_spiral_edge_cases = true, - const bool with_signals = true); + const bool with_road_signals = true); std::vector get_roads() const; std::vector get_junctions() const; diff --git a/include/Road.h b/include/Road.h index c544b8d..da218ec 100644 --- a/include/Road.h +++ b/include/Road.h @@ -5,7 +5,7 @@ #include "Mesh.h" #include "RefLine.h" #include "RoadObject.h" -#include "Signal.h" +#include "RoadSignal.h" #include "XmlNode.h" #include @@ -83,7 +83,7 @@ class Road : public XmlNode std::vector get_lanesections() const; std::vector get_road_objects() const; - std::vector get_signals() const; + std::vector get_road_signals() const; double get_lanesection_s0(const double s) const; LaneSection get_lanesection(const double s) const; @@ -104,7 +104,7 @@ class Road : public XmlNode Mesh3D get_lane_mesh(const Lane& lane, const double eps, std::vector* outline_indices = nullptr) const; Mesh3D get_roadmark_mesh(const Lane& lane, const RoadMark& roadmark, const double eps) const; - Mesh3D get_signal_mesh(const Signal& signal) const; + Mesh3D get_road_signal_mesh(const RoadSignal& road_signal) const; Mesh3D get_road_object_mesh(const RoadObject& road_object, const double eps) const; std::set @@ -130,7 +130,7 @@ class Road : public XmlNode std::map s_to_type; std::map s_to_speed; std::map id_to_object; - std::map id_to_signal; + std::map id_to_signal; }; } // namespace odr diff --git a/include/RoadNetworkMesh.h b/include/RoadNetworkMesh.h index f3b9c66..efe3d77 100644 --- a/include/RoadNetworkMesh.h +++ b/include/RoadNetworkMesh.h @@ -56,12 +56,12 @@ struct RoadObjectsMesh : public RoadsMesh std::map road_object_start_indices; }; -struct SignalsMesh : public RoadsMesh +struct RoadSignalsMesh : public RoadsMesh { - std::string get_signal_id(const std::size_t vert_idx) const; + std::string get_road_signal_id(const std::size_t vert_idx) const; std::array get_idx_interval_signal(const std::size_t vert_idx) const; - std::map signal_start_indices; + std::map road_signal_start_indices; }; struct RoadNetworkMesh @@ -71,7 +71,7 @@ struct RoadNetworkMesh LanesMesh lanes_mesh; RoadmarksMesh roadmarks_mesh; RoadObjectsMesh road_objects_mesh; - SignalsMesh signals_mesh; + RoadSignalsMesh road_signals_mesh; }; } // namespace odr \ No newline at end of file diff --git a/include/Signal.h b/include/RoadSignal.h similarity index 54% rename from include/Signal.h rename to include/RoadSignal.h index 0bdbb0f..195f8f4 100644 --- a/include/Signal.h +++ b/include/RoadSignal.h @@ -9,27 +9,27 @@ namespace odr { -struct Signal : public XmlNode +struct RoadSignal : public XmlNode { - Signal(std::string road_id, - std::string id, - std::string name, - double s0, - double t0, - bool is_dynamic, - double zOffset, - double value, - double height, - double width, - double hOffset, - double pitch, - double roll, - std::string orientation, - std::string country, - std::string type, - std::string subtype, - std::string unit, - std::string text); + RoadSignal(std::string road_id, + std::string id, + std::string name, + double s0, + double t0, + bool is_dynamic, + double zOffset, + double value, + double height, + double width, + double hOffset, + double pitch, + double roll, + std::string orientation, + std::string country, + std::string type, + std::string subtype, + std::string unit, + std::string text); static Mesh3D get_box(const double width, const double length, const double height); diff --git a/src/OpenDriveMap.cpp b/src/OpenDriveMap.cpp index c160bb4..a39de30 100644 --- a/src/OpenDriveMap.cpp +++ b/src/OpenDriveMap.cpp @@ -14,7 +14,7 @@ #include "Road.h" #include "RoadMark.h" #include "RoadObject.h" -#include "Signal.h" +#include "RoadSignal.h" #include "Utils.hpp" #include @@ -61,7 +61,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file, const bool with_lane_height, const bool abs_z_for_for_local_road_obj_outline, const bool fix_spiral_edge_cases, - const bool with_signals) : + const bool with_road_signals) : xodr_file(xodr_file) { pugi::xml_parse_result result = this->xml_doc.load_file(xodr_file.c_str()); @@ -632,44 +632,44 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file, } } /* parse signals */ - if (with_signals) + if (with_road_signals) { for (pugi::xml_node signal_node : road_node.child("signals").children("signal")) { - std::string signal_id = signal_node.attribute("id").as_string(""); - CHECK_AND_REPAIR(road.id_to_signal.find(signal_id) == road.id_to_signal.end(), - (std::string("signal::id already exists - ") + signal_id).c_str(), - signal_id = signal_id + std::string("_dup")); - - Signal& signal = road.id_to_signal - .insert({signal_id, - Signal(road_id, - signal_id, - signal_node.attribute("name").as_string(""), - signal_node.attribute("s").as_double(0), - signal_node.attribute("t").as_double(0), - signal_node.attribute("dynamic").as_bool(), - signal_node.attribute("zOffset").as_double(0), - signal_node.attribute("value").as_double(0), - signal_node.attribute("height").as_double(0), - signal_node.attribute("width").as_double(0), - signal_node.attribute("hOffset").as_double(0), - signal_node.attribute("pitch").as_double(0), - signal_node.attribute("roll").as_double(0), - signal_node.attribute("orientation").as_string("none"), - signal_node.attribute("country").as_string(""), - signal_node.attribute("type").as_string("none"), - signal_node.attribute("subtype").as_string("none"), - signal_node.attribute("unit").as_string(""), - signal_node.attribute("text").as_string("none"))}) - .first->second; - signal.xml_node = signal_node; - - CHECK_AND_REPAIR(signal.s0 >= 0, "signal::s < 0", signal.s0 = 0); - CHECK_AND_REPAIR(signal.height >= 0, "signal::height < 0", signal.height = 0); - CHECK_AND_REPAIR(signal.width >= 0, "signal::width < 0", signal.width = 0); - - signal.lane_validities = extract_lane_validity_records(signal_node); + std::string road_signal_id = signal_node.attribute("id").as_string(""); + CHECK_AND_REPAIR(road.id_to_signal.find(road_signal_id) == road.id_to_signal.end(), + (std::string("signal::id already exists - ") + road_signal_id).c_str(), + road_signal_id = road_signal_id + std::string("_dup")); + + RoadSignal& road_signal = road.id_to_signal + .insert({road_signal_id, + RoadSignal(road_id, + road_signal_id, + signal_node.attribute("name").as_string(""), + signal_node.attribute("s").as_double(0), + signal_node.attribute("t").as_double(0), + signal_node.attribute("dynamic").as_bool(), + signal_node.attribute("zOffset").as_double(0), + signal_node.attribute("value").as_double(0), + signal_node.attribute("height").as_double(0), + signal_node.attribute("width").as_double(0), + signal_node.attribute("hOffset").as_double(0), + signal_node.attribute("pitch").as_double(0), + signal_node.attribute("roll").as_double(0), + signal_node.attribute("orientation").as_string("none"), + signal_node.attribute("country").as_string(""), + signal_node.attribute("type").as_string("none"), + signal_node.attribute("subtype").as_string("none"), + signal_node.attribute("unit").as_string(""), + signal_node.attribute("text").as_string("none"))}) + .first->second; + road_signal.xml_node = signal_node; + + CHECK_AND_REPAIR(road_signal.s0 >= 0, "signal::s < 0", road_signal.s0 = 0); + CHECK_AND_REPAIR(road_signal.height >= 0, "signal::height < 0", road_signal.height = 0); + CHECK_AND_REPAIR(road_signal.width >= 0, "signal::width < 0", road_signal.width = 0); + + road_signal.lane_validities = extract_lane_validity_records(signal_node); } } } @@ -685,7 +685,7 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const LanesMesh& lanes_mesh = out_mesh.lanes_mesh; RoadmarksMesh& roadmarks_mesh = out_mesh.roadmarks_mesh; RoadObjectsMesh& road_objects_mesh = out_mesh.road_objects_mesh; - SignalsMesh& signals_mesh = out_mesh.signals_mesh; + RoadSignalsMesh& road_signals_mesh = out_mesh.road_signals_mesh; for (const auto& id_road : this->id_to_road) { @@ -728,10 +728,10 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const for (const auto& id_signal : road.id_to_signal) { - const Signal& signal = id_signal.second; - const std::size_t signals_idx_offset = signals_mesh.vertices.size(); - signals_mesh.signal_start_indices[signals_idx_offset] = signal.id; - signals_mesh.add_mesh(road.get_signal_mesh(signal)); + const RoadSignal& road_signal = id_signal.second; + const std::size_t signals_idx_offset = road_signals_mesh.vertices.size(); + road_signals_mesh.road_signal_start_indices[signals_idx_offset] = road_signal.id; + road_signals_mesh.add_mesh(road.get_road_signal_mesh(road_signal)); } } diff --git a/src/Road.cpp b/src/Road.cpp index c4b701c..4578af1 100644 --- a/src/Road.cpp +++ b/src/Road.cpp @@ -52,7 +52,7 @@ SpeedRecord::SpeedRecord(std::string max, std::string unit) : max(max), unit(uni std::vector Road::get_lanesections() const { return get_map_values(this->s_to_lanesection); } std::vector Road::get_road_objects() const { return get_map_values(this->id_to_object); } -std::vector Road::get_signals() const { return get_map_values(this->id_to_signal); } +std::vector Road::get_road_signals() const { return get_map_values(this->id_to_signal); } Road::Road(std::string id, double length, std::string junction, std::string name, bool left_hand_traffic) : length(length), id(id), junction(junction), name(name), left_hand_traffic(left_hand_traffic), ref_line(id, length) @@ -334,28 +334,28 @@ Mesh3D Road::get_roadmark_mesh(const Lane& lane, const RoadMark& roadmark, const return out_mesh; } -Mesh3D Road::get_signal_mesh(const Signal& signal) const +Mesh3D Road::get_road_signal_mesh(const RoadSignal& road_signal) const { - const Mat3D rot_mat = EulerAnglesToMatrix(signal.roll, signal.pitch, signal.hOffset); - const double s = signal.s0; - const double t = signal.t0; - const double z = signal.zOffset; - const double height = signal.height; - const double width = signal.width; - Mesh3D signal_mesh; - signal_mesh = signal.get_box(width, 0.2, height); + const Mat3D rot_mat = EulerAnglesToMatrix(road_signal.roll, road_signal.pitch, road_signal.hOffset); + const double s = road_signal.s0; + const double t = road_signal.t0; + const double z = road_signal.zOffset; + const double height = road_signal.height; + const double width = road_signal.width; + Mesh3D road_signal_mesh; + road_signal_mesh = road_signal.get_box(width, 0.2, height); Vec3D e_s, e_t, e_h; const Vec3D p0 = this->get_xyz(s, t, z, &e_s, &e_t, &e_h); const Mat3D base_mat{{{e_s[0], e_t[0], e_h[0]}, {e_s[1], e_t[1], e_h[1]}, {e_s[2], e_t[2], e_h[2]}}}; - for (Vec3D& pt_uvz : signal_mesh.vertices) + for (Vec3D& pt_uvz : road_signal_mesh.vertices) { pt_uvz = MatVecMultiplication(rot_mat, pt_uvz); pt_uvz = MatVecMultiplication(base_mat, pt_uvz); pt_uvz = add(pt_uvz, p0); - signal_mesh.st_coordinates.push_back({s, t}); + road_signal_mesh.st_coordinates.push_back({s, t}); } - return signal_mesh; + return road_signal_mesh; } Mesh3D Road::get_road_object_mesh(const RoadObject& road_object, const double eps) const diff --git a/src/RoadNetworkMesh.cpp b/src/RoadNetworkMesh.cpp index 9b9b221..5c39adb 100644 --- a/src/RoadNetworkMesh.cpp +++ b/src/RoadNetworkMesh.cpp @@ -84,14 +84,14 @@ std::array RoadObjectsMesh::get_idx_interval_road_object(const std::s return get_key_interval(this->road_object_start_indices, vert_idx, this->vertices.size()); } -std::string SignalsMesh::get_signal_id(const std::size_t vert_idx) const +std::string RoadSignalsMesh::get_road_signal_id(const std::size_t vert_idx) const { - return get_nearest_lower_val(this->signal_start_indices, vert_idx); + return get_nearest_lower_val(this->road_signal_start_indices, vert_idx); } -std::array SignalsMesh::get_idx_interval_signal(const std::size_t vert_idx) const +std::array RoadSignalsMesh::get_idx_interval_signal(const std::size_t vert_idx) const { - return get_key_interval(this->signal_start_indices, vert_idx, this->vertices.size()); + return get_key_interval(this->road_signal_start_indices, vert_idx, this->vertices.size()); } Mesh3D RoadNetworkMesh::get_mesh() const @@ -100,7 +100,7 @@ Mesh3D RoadNetworkMesh::get_mesh() const out_mesh.add_mesh(this->lanes_mesh); out_mesh.add_mesh(this->roadmarks_mesh); out_mesh.add_mesh(this->road_objects_mesh); - out_mesh.add_mesh(this->signals_mesh); + out_mesh.add_mesh(this->road_signals_mesh); return out_mesh; } diff --git a/src/Signal.cpp b/src/RoadSignal.cpp similarity index 50% rename from src/Signal.cpp rename to src/RoadSignal.cpp index 93a226c..cdd1fa0 100644 --- a/src/Signal.cpp +++ b/src/RoadSignal.cpp @@ -1,33 +1,33 @@ -#include "Signal.h" +#include "RoadSignal.h" namespace odr { -Signal::Signal(std::string road_id, - std::string id, - std::string name, - double s0, - double t0, - bool is_dynamic, - double zOffset, - double value, - double height, - double width, - double hOffset, - double pitch, - double roll, - std::string orientation, - std::string country, - std::string type, - std::string subtype, - std::string unit, - std::string text) : +RoadSignal::RoadSignal(std::string road_id, + std::string id, + std::string name, + double s0, + double t0, + bool is_dynamic, + double zOffset, + double value, + double height, + double width, + double hOffset, + double pitch, + double roll, + std::string orientation, + std::string country, + std::string type, + std::string subtype, + std::string unit, + std::string text) : road_id(road_id), id(id), name(name), s0(s0), t0(t0), is_dynamic(is_dynamic), zOffset(zOffset), value(value), height(height), width(width), hOffset(hOffset), pitch(pitch), roll(roll), orientation(orientation), country(country), type(type), subtype(subtype), unit(unit), text(text) { } -Mesh3D Signal::get_box(const double w, const double l, const double h) +Mesh3D RoadSignal::get_box(const double w, const double l, const double h) { Mesh3D box_mesh; box_mesh.vertices = {Vec3D{l / 2, w / 2, 0}, diff --git a/test.cpp b/test.cpp index 81def5b..c34a557 100644 --- a/test.cpp +++ b/test.cpp @@ -23,7 +23,7 @@ int main(int argc, char** argv) std::vector lane_pts; std::vector roadmark_pts; std::vector road_object_pts; - std::vector signal_pts; + std::vector road_signal_pts; for (odr::Road road : odr_map.get_roads()) { @@ -53,15 +53,15 @@ int main(int argc, char** argv) road_object_pts.insert(road_object_pts.end(), road_object_mesh.vertices.begin(), road_object_mesh.vertices.end()); } - for (odr::Signal signal: road.get_signals()) + for (odr::RoadSignal road_signal: road.get_road_signals()) { - auto signal_mesh = road.get_signal_mesh(signal); - signal_pts.insert(signal_pts.end(), signal_mesh.vertices.begin(), signal_mesh.vertices.end()); + auto road_signal_mesh = road.get_road_signal_mesh(road_signal); + road_signal_pts.insert(road_signal_pts.end(), road_signal_mesh.vertices.begin(), road_signal_mesh.vertices.end()); } } - printf("Finished, got %llu lane points, %llu roadmark points, %llu road object points, %llu signal points\n", - lane_pts.size(), roadmark_pts.size(), road_object_pts.size(), signal_pts.size()); + printf("Finished, got %llu lane points, %llu roadmark points, %llu road object points, %llu road signal points\n", + lane_pts.size(), roadmark_pts.size(), road_object_pts.size(), road_signal_pts.size()); odr::RoadNetworkMesh road_network_mesh = odr_map.get_road_network_mesh(eps); printf("Got road network mesh\n");