Skip to content

Commit d88e6ab

Browse files
pageldevpageldev
authored
Merge pull request #84 from DLR-TS/feature/signals_implementation
Feature: add signal mesh creation
2 parents 212aa34 + 11fcb21 commit d88e6ab

File tree

12 files changed

+217
-131
lines changed

12 files changed

+217
-131
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -36,7 +36,7 @@ add_library(OpenDrive SHARED
3636
src/RoadMark.cpp
3737
src/RoadNetworkMesh.cpp
3838
src/RoadObject.cpp
39-
src/Signal.cpp
39+
src/RoadSignal.cpp
4040
src/RoutingGraph.cpp
4141
thirdparty/pugixml/pugixml.cpp
4242
)

‎include/OpenDriveMap.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,7 @@ class OpenDriveMap
2323
const bool with_lane_height = true,
2424
const bool abs_z_for_for_local_road_obj_outline = false,
2525
const bool fix_spiral_edge_cases = true,
26-
const bool with_signals = true);
26+
const bool with_road_signals = true);
2727

2828
std::vector<Road> get_roads() const;
2929
std::vector<Junction> get_junctions() const;

‎include/Road.h‎

Lines changed: 4 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -5,7 +5,7 @@
55
#include "Mesh.h"
66
#include "RefLine.h"
77
#include "RoadObject.h"
8-
#include "Signal.h"
8+
#include "RoadSignal.h"
99
#include "XmlNode.h"
1010

1111
#include <cstddef>
@@ -83,7 +83,7 @@ class Road : public XmlNode
8383

8484
std::vector<LaneSection> get_lanesections() const;
8585
std::vector<RoadObject> get_road_objects() const;
86-
std::vector<Signal> get_signals() const;
86+
std::vector<RoadSignal> get_road_signals() const;
8787

8888
double get_lanesection_s0(const double s) const;
8989
LaneSection get_lanesection(const double s) const;
@@ -104,6 +104,7 @@ class Road : public XmlNode
104104
Mesh3D get_lane_mesh(const Lane& lane, const double eps, std::vector<uint32_t>* outline_indices = nullptr) const;
105105

106106
Mesh3D get_roadmark_mesh(const Lane& lane, const RoadMark& roadmark, const double eps) const;
107+
Mesh3D get_road_signal_mesh(const RoadSignal& road_signal) const;
107108
Mesh3D get_road_object_mesh(const RoadObject& road_object, const double eps) const;
108109

109110
std::set<double>
@@ -129,7 +130,7 @@ class Road : public XmlNode
129130
std::map<double, std::string> s_to_type;
130131
std::map<double, SpeedRecord> s_to_speed;
131132
std::map<std::string, RoadObject> id_to_object;
132-
std::map<std::string, Signal> id_to_signal;
133+
std::map<std::string, RoadSignal> id_to_signal;
133134
};
134135

135136
} // namespace odr

‎include/RoadNetworkMesh.h‎

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -56,13 +56,22 @@ struct RoadObjectsMesh : public RoadsMesh
5656
std::map<size_t, std::string> road_object_start_indices;
5757
};
5858

59+
struct RoadSignalsMesh : public RoadsMesh
60+
{
61+
std::string get_road_signal_id(const std::size_t vert_idx) const;
62+
std::array<size_t, 2> get_idx_interval_signal(const std::size_t vert_idx) const;
63+
64+
std::map<size_t, std::string> road_signal_start_indices;
65+
};
66+
5967
struct RoadNetworkMesh
6068
{
6169
Mesh3D get_mesh() const;
6270

6371
LanesMesh lanes_mesh;
6472
RoadmarksMesh roadmarks_mesh;
6573
RoadObjectsMesh road_objects_mesh;
74+
RoadSignalsMesh road_signals_mesh;
6675
};
6776

6877
} // namespace odr

‎include/RoadSignal.h‎

Lines changed: 59 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
#pragma once
2+
#include "LaneValidityRecord.h"
3+
#include "Mesh.h"
4+
#include "XmlNode.h"
5+
6+
#include <string>
7+
#include <vector>
8+
9+
namespace odr
10+
{
11+
12+
struct RoadSignal : public XmlNode
13+
{
14+
RoadSignal(std::string road_id,
15+
std::string id,
16+
std::string name,
17+
double s0,
18+
double t0,
19+
bool is_dynamic,
20+
double zOffset,
21+
double value,
22+
double height,
23+
double width,
24+
double hOffset,
25+
double pitch,
26+
double roll,
27+
std::string orientation,
28+
std::string country,
29+
std::string type,
30+
std::string subtype,
31+
std::string unit,
32+
std::string text);
33+
34+
static Mesh3D get_box(const double width, const double length, const double height);
35+
36+
std::string road_id = "";
37+
std::string id = "";
38+
std::string name = "";
39+
double s0 = 0;
40+
double t0 = 0;
41+
bool is_dynamic = false;
42+
double zOffset = 0;
43+
double value = 0;
44+
double height = 0;
45+
double width = 0;
46+
double hOffset = 0;
47+
double pitch = 0;
48+
double roll = 0;
49+
std::string orientation = "";
50+
std::string country = "";
51+
std::string type = "";
52+
std::string subtype = "";
53+
std::string unit = "";
54+
std::string text = "";
55+
56+
std::vector<LaneValidityRecord> lane_validities;
57+
};
58+
59+
} // namespace odr

‎include/Signal.h‎

Lines changed: 0 additions & 56 deletions
This file was deleted.

‎src/OpenDriveMap.cpp‎

Lines changed: 46 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@
1414
#include "Road.h"
1515
#include "RoadMark.h"
1616
#include "RoadObject.h"
17-
#include "Signal.h"
17+
#include "RoadSignal.h"
1818
#include "Utils.hpp"
1919

2020
#include <algorithm>
@@ -61,7 +61,7 @@ OpenDriveMap::OpenDriveMap(const std::string& xodr_file,
6161
const bool with_lane_height,
6262
const bool abs_z_for_for_local_road_obj_outline,
6363
const bool fix_spiral_edge_cases,
64-
const bool with_signals) :
64+
const bool with_road_signals) :
6565
xodr_file(xodr_file)
6666
{
6767
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,
632632
}
633633
}
634634
/* parse signals */
635-
if (with_signals)
635+
if (with_road_signals)
636636
{
637637
for (pugi::xml_node signal_node : road_node.child("signals").children("signal"))
638638
{
639-
std::string signal_id = signal_node.attribute("id").as_string("");
640-
CHECK_AND_REPAIR(road.id_to_signal.find(signal_id) == road.id_to_signal.end(),
641-
(std::string("signal::id already exists - ") + signal_id).c_str(),
642-
signal_id = signal_id + std::string("_dup"));
643-
644-
const bool is_dynamic_signal = std::string(signal_node.attribute("dynamic").as_string("no")) == "yes";
645-
Signal& signal = road.id_to_signal
646-
.insert({signal_id,
647-
Signal(road_id,
648-
signal_id,
649-
signal_node.attribute("name").as_string(""),
650-
signal_node.attribute("s").as_double(0),
651-
signal_node.attribute("t").as_double(0),
652-
is_dynamic_signal,
653-
signal_node.attribute("zOffset").as_double(0),
654-
signal_node.attribute("value").as_double(0),
655-
signal_node.attribute("height").as_double(0),
656-
signal_node.attribute("width").as_double(0),
657-
signal_node.attribute("hOffset").as_double(0),
658-
signal_node.attribute("pitch").as_double(0),
659-
signal_node.attribute("roll").as_double(0),
660-
signal_node.attribute("orientation").as_string("none"),
661-
signal_node.attribute("country").as_string(""),
662-
signal_node.attribute("type").as_string("none"),
663-
signal_node.attribute("subtype").as_string("none"),
664-
signal_node.attribute("unit").as_string(""),
665-
signal_node.attribute("text").as_string("none"))})
666-
.first->second;
667-
signal.xml_node = signal_node;
668-
669-
CHECK_AND_REPAIR(signal.s0 >= 0, "signal::s < 0", signal.s0 = 0);
670-
CHECK_AND_REPAIR(signal.height >= 0, "signal::height < 0", signal.height = 0);
671-
CHECK_AND_REPAIR(signal.width >= 0, "signal::width < 0", signal.width = 0);
672-
673-
signal.lane_validities = extract_lane_validity_records(signal_node);
639+
std::string road_signal_id = signal_node.attribute("id").as_string("");
640+
CHECK_AND_REPAIR(road.id_to_signal.find(road_signal_id) == road.id_to_signal.end(),
641+
(std::string("signal::id already exists - ") + road_signal_id).c_str(),
642+
road_signal_id = road_signal_id + std::string("_dup"));
643+
644+
RoadSignal& road_signal = road.id_to_signal
645+
.insert({road_signal_id,
646+
RoadSignal(road_id,
647+
road_signal_id,
648+
signal_node.attribute("name").as_string(""),
649+
signal_node.attribute("s").as_double(0),
650+
signal_node.attribute("t").as_double(0),
651+
signal_node.attribute("dynamic").as_bool(),
652+
signal_node.attribute("zOffset").as_double(0),
653+
signal_node.attribute("value").as_double(0),
654+
signal_node.attribute("height").as_double(0),
655+
signal_node.attribute("width").as_double(0),
656+
signal_node.attribute("hOffset").as_double(0),
657+
signal_node.attribute("pitch").as_double(0),
658+
signal_node.attribute("roll").as_double(0),
659+
signal_node.attribute("orientation").as_string("none"),
660+
signal_node.attribute("country").as_string(""),
661+
signal_node.attribute("type").as_string("none"),
662+
signal_node.attribute("subtype").as_string("none"),
663+
signal_node.attribute("unit").as_string(""),
664+
signal_node.attribute("text").as_string("none"))})
665+
.first->second;
666+
road_signal.xml_node = signal_node;
667+
668+
CHECK_AND_REPAIR(road_signal.s0 >= 0, "signal::s < 0", road_signal.s0 = 0);
669+
CHECK_AND_REPAIR(road_signal.height >= 0, "signal::height < 0", road_signal.height = 0);
670+
CHECK_AND_REPAIR(road_signal.width >= 0, "signal::width < 0", road_signal.width = 0);
671+
672+
road_signal.lane_validities = extract_lane_validity_records(signal_node);
674673
}
675674
}
676675
}
@@ -686,6 +685,7 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const
686685
LanesMesh& lanes_mesh = out_mesh.lanes_mesh;
687686
RoadmarksMesh& roadmarks_mesh = out_mesh.roadmarks_mesh;
688687
RoadObjectsMesh& road_objects_mesh = out_mesh.road_objects_mesh;
688+
RoadSignalsMesh& road_signals_mesh = out_mesh.road_signals_mesh;
689689

690690
for (const auto& id_road : this->id_to_road)
691691
{
@@ -725,6 +725,14 @@ RoadNetworkMesh OpenDriveMap::get_road_network_mesh(const double eps) const
725725
road_objects_mesh.road_object_start_indices[road_objs_idx_offset] = road_object.id;
726726
road_objects_mesh.add_mesh(road.get_road_object_mesh(road_object, eps));
727727
}
728+
729+
for (const auto& id_signal : road.id_to_signal)
730+
{
731+
const RoadSignal& road_signal = id_signal.second;
732+
const std::size_t signals_idx_offset = road_signals_mesh.vertices.size();
733+
road_signals_mesh.road_signal_start_indices[signals_idx_offset] = road_signal.id;
734+
road_signals_mesh.add_mesh(road.get_road_signal_mesh(road_signal));
735+
}
728736
}
729737

730738
return out_mesh;

‎src/Road.cpp‎

Lines changed: 25 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -50,10 +50,9 @@ RoadNeighbor::RoadNeighbor(std::string id, std::string side, std::string directi
5050
SpeedRecord::SpeedRecord(std::string max, std::string unit) : max(max), unit(unit) {}
5151

5252
std::vector<LaneSection> Road::get_lanesections() const { return get_map_values(this->s_to_lanesection); }
53-
5453
std::vector<RoadObject> Road::get_road_objects() const { return get_map_values(this->id_to_object); }
5554

56-
std::vector<Signal> Road::get_signals() const { return get_map_values(this->id_to_signal); }
55+
std::vector<RoadSignal> Road::get_road_signals() const { return get_map_values(this->id_to_signal); }
5756

5857
Road::Road(std::string id, double length, std::string junction, std::string name, bool left_hand_traffic) :
5958
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
335334
return out_mesh;
336335
}
337336

337+
Mesh3D Road::get_road_signal_mesh(const RoadSignal& road_signal) const
338+
{
339+
const Mat3D rot_mat = EulerAnglesToMatrix<double>(road_signal.roll, road_signal.pitch, road_signal.hOffset);
340+
const double s = road_signal.s0;
341+
const double t = road_signal.t0;
342+
const double z = road_signal.zOffset;
343+
const double height = road_signal.height;
344+
const double width = road_signal.width;
345+
Mesh3D road_signal_mesh;
346+
road_signal_mesh = road_signal.get_box(width, 0.2, height);
347+
Vec3D e_s, e_t, e_h;
348+
const Vec3D p0 = this->get_xyz(s, t, z, &e_s, &e_t, &e_h);
349+
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]}}};
350+
for (Vec3D& pt_uvz : road_signal_mesh.vertices)
351+
{
352+
pt_uvz = MatVecMultiplication(rot_mat, pt_uvz);
353+
pt_uvz = MatVecMultiplication(base_mat, pt_uvz);
354+
pt_uvz = add(pt_uvz, p0);
355+
356+
road_signal_mesh.st_coordinates.push_back({s, t});
357+
}
358+
return road_signal_mesh;
359+
}
360+
338361
Mesh3D Road::get_road_object_mesh(const RoadObject& road_object, const double eps) const
339362
{
340363
std::vector<RoadObjectRepeat> repeats_copy = road_object.repeats; // make copy to keep method const

‎src/RoadNetworkMesh.cpp‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -84,12 +84,23 @@ std::array<size_t, 2> RoadObjectsMesh::get_idx_interval_road_object(const std::s
8484
return get_key_interval<size_t, std::string>(this->road_object_start_indices, vert_idx, this->vertices.size());
8585
}
8686

87+
std::string RoadSignalsMesh::get_road_signal_id(const std::size_t vert_idx) const
88+
{
89+
return get_nearest_lower_val<size_t, std::string>(this->road_signal_start_indices, vert_idx);
90+
}
91+
92+
std::array<size_t, 2> RoadSignalsMesh::get_idx_interval_signal(const std::size_t vert_idx) const
93+
{
94+
return get_key_interval<size_t, std::string>(this->road_signal_start_indices, vert_idx, this->vertices.size());
95+
}
96+
8797
Mesh3D RoadNetworkMesh::get_mesh() const
8898
{
8999
Mesh3D out_mesh;
90100
out_mesh.add_mesh(this->lanes_mesh);
91101
out_mesh.add_mesh(this->roadmarks_mesh);
92102
out_mesh.add_mesh(this->road_objects_mesh);
103+
out_mesh.add_mesh(this->road_signals_mesh);
93104
return out_mesh;
94105
}
95106

0 commit comments

Comments
 (0)