Merge pull request #84 from DLR-TS/feature/signals_implementation

Feature: add signal mesh creation
pageldev · Sep 13, 2023 · d88e6ab · d88e6ab
2 parents 212aa34 + 11fcb21
commit d88e6ab
Show file tree

Hide file tree

Showing 12 changed files with 217 additions and 131 deletions.
diff --git 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
@@ -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<Road>     get_roads() const;
     std::vector<Junction> get_junctions() const;

diff --git 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 <cstddef>
@@ -83,7 +83,7 @@ class Road : public XmlNode
 
     std::vector<LaneSection> get_lanesections() const;
     std::vector<RoadObject>  get_road_objects() const;
-    std::vector<Signal>      get_signals() const;
+    std::vector<RoadSignal>  get_road_signals() const;
 
     double      get_lanesection_s0(const double s) const;
     LaneSection get_lanesection(const double s) const;
@@ -104,6 +104,7 @@ class Road : public XmlNode
     Mesh3D get_lane_mesh(const Lane& lane, const double eps, std::vector<uint32_t>* 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<double>
@@ -129,7 +130,7 @@ class Road : public XmlNode
     std::map<double, std::string>     s_to_type;
     std::map<double, SpeedRecord>     s_to_speed;
     std::map<std::string, RoadObject> id_to_object;
-    std::map<std::string, Signal>     id_to_signal;
+    std::map<std::string, RoadSignal> id_to_signal;
 };
 
 } // namespace odr
diff --git a/include/RoadNetworkMesh.h b/include/RoadNetworkMesh.h
@@ -56,13 +56,22 @@ struct RoadObjectsMesh : public RoadsMesh
     std::map<size_t, std::string> road_object_start_indices;
 };
 
+struct RoadSignalsMesh : public RoadsMesh
+{
+    std::string           get_road_signal_id(const std::size_t vert_idx) const;
+    std::array<size_t, 2> get_idx_interval_signal(const std::size_t vert_idx) const;
+
+    std::map<size_t, std::string> road_signal_start_indices;
+};
+
 struct RoadNetworkMesh
 {
     Mesh3D get_mesh() const;
 
     LanesMesh       lanes_mesh;
     RoadmarksMesh   roadmarks_mesh;
     RoadObjectsMesh road_objects_mesh;
+    RoadSignalsMesh road_signals_mesh;
 };
 
 } // namespace odr
diff --git a/include/RoadSignal.h b/include/RoadSignal.h
@@ -0,0 +1,59 @@
+#pragma once
+#include "LaneValidityRecord.h"
+#include "Mesh.h"
+#include "XmlNode.h"
+
+#include <string>
+#include <vector>
+
+namespace odr
+{
+
+struct RoadSignal : public XmlNode
+{
+    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);
+
+    std::string road_id = "";
+    std::string id = "";
+    std::string name = "";
+    double      s0 = 0;
+    double      t0 = 0;
+    bool        is_dynamic = false;
+    double      zOffset = 0;
+    double      value = 0;
+    double      height = 0;
+    double      width = 0;
+    double      hOffset = 0;
+    double      pitch = 0;
+    double      roll = 0;
+    std::string orientation = "";
+    std::string country = "";
+    std::string type = "";
+    std::string subtype = "";
+    std::string unit = "";
+    std::string text = "";
+
+    std::vector<LaneValidityRecord> lane_validities;
+};
+
+} // namespace odr
diff --git a/include/Signal.h b/include/Signal.h
diff --git 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 <algorithm>
@@ -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,45 +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"));
-
-                const bool is_dynamic_signal = std::string(signal_node.attribute("dynamic").as_string("no")) == "yes";
-                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),
-                                                     is_dynamic_signal,
-                                                     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);
             }
         }
     }
@@ -686,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;
+    RoadSignalsMesh& road_signals_mesh = out_mesh.road_signals_mesh;
 
     for (const auto& id_road : this->id_to_road)
     {
@@ -725,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 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));
+        }
     }
 
     return out_mesh;

diff --git a/src/Road.cpp b/src/Road.cpp
@@ -50,10 +50,9 @@ 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<LaneSection> Road::get_lanesections() const { return get_map_values(this->s_to_lanesection); }
-
 std::vector<RoadObject> Road::get_road_objects() const { return get_map_values(this->id_to_object); }
 
-std::vector<Signal> Road::get_signals() const { return get_map_values(this->id_to_signal); }
+std::vector<RoadSignal> 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)
@@ -335,6 +334,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<double>(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 : 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<RoadObjectRepeat> repeats_copy = road_object.repeats; // make copy to keep method const

diff --git a/src/RoadNetworkMesh.cpp b/src/RoadNetworkMesh.cpp
@@ -84,12 +84,23 @@ std::array<size_t, 2> RoadObjectsMesh::get_idx_interval_road_object(const std::s
     return get_key_interval<size_t, std::string>(this->road_object_start_indices, vert_idx, this->vertices.size());
 }
 
+std::string RoadSignalsMesh::get_road_signal_id(const std::size_t vert_idx) const
+{
+    return get_nearest_lower_val<size_t, std::string>(this->road_signal_start_indices, vert_idx);
+}
+
+std::array<size_t, 2> RoadSignalsMesh::get_idx_interval_signal(const std::size_t vert_idx) const
+{
+    return get_key_interval<size_t, std::string>(this->road_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->road_signals_mesh);
     return out_mesh;
 }