CARLA
Road.cpp
Go to the documentation of this file.
1 // Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #include "carla/Exception.h"
9 #include "carla/geom/Location.h"
10 #include "carla/geom/Math.h"
11 #include "carla/ListView.h"
12 #include "carla/Logging.h"
17 #include "carla/road/Lane.h"
18 #include "carla/road/MapData.h"
19 #include "carla/road/Road.h"
20 
21 #include <stdexcept>
22 
23 namespace carla {
24 namespace road {
25 
26  const MapData *Road::GetMap() const {
27  return _map_data;
28  }
29 
30  RoadId Road::GetId() const {
31  return _id;
32  }
33 
34  std::string Road::GetName() const {
35  return _name;
36  }
37 
38  double Road::GetLength() const {
39  return _length;
40  }
41 
42  bool Road::IsJunction() const {
43  return _is_junction;
44  }
45 
47  return _junction_id;
48  }
49 
51  return _successor;
52  }
53 
55  return _predecessor;
56  }
57 
58  std::vector<Road *> Road::GetNexts() const {
59  return _nexts;
60  }
61 
62  std::vector<Road *> Road::GetPrevs() const {
63  return _prevs;
64  }
65 
66  const geom::CubicPolynomial &Road::GetElevationOn(const double s) const {
67  auto info = GetInfo<element::RoadInfoElevation>(s);
68  if (info == nullptr) {
69  throw_exception(std::runtime_error("failed to find road elevation."));
70  }
71  return info->GetPolynomial();
72  }
73 
74  Lane &Road::GetLaneByDistance(double s, LaneId lane_id) {
75  for (auto &section : GetLaneSectionsAt(s)) {
76  auto *lane = section.GetLane(lane_id);
77  if (lane != nullptr) {
78  return *lane;
79  }
80  }
81  throw_exception(std::runtime_error("lane not found"));
82  }
83 
84  const Lane &Road::GetLaneByDistance(double s, LaneId lane_id) const {
85  return const_cast<Road *>(this)->GetLaneByDistance(s, lane_id);
86  }
87 
88  std::vector<Lane*> Road::GetLanesByDistance(double s) {
89  std::vector<Lane*> result;
90  auto lane_sections = GetLaneSectionsAt(s);
91  for (auto &lane_section : lane_sections) {
92  for (auto & lane_pair : lane_section.GetLanes()) {
93  result.emplace_back(&lane_pair.second);
94  }
95  }
96  return result;
97  }
98 
99  std::vector<const Lane*> Road::GetLanesByDistance(double s) const {
100  std::vector<const Lane*> result;
101  const auto lane_sections = GetLaneSectionsAt(s);
102  for (const auto &lane_section : lane_sections) {
103  for (const auto & lane_pair : lane_section.GetLanes()) {
104  result.emplace_back(&lane_pair.second);
105  }
106  }
107  return result;
108  }
109 
110  Lane &Road::GetLaneById(SectionId section_id, LaneId lane_id) {
111  return GetLaneSectionById(section_id).GetLanes().at(lane_id);
112  }
113 
114  const Lane &Road::GetLaneById(SectionId section_id, LaneId lane_id) const {
115  return const_cast<Road *>(this)->GetLaneById(section_id, lane_id);
116  }
117 
118  // get the lane on a section next to 's'
119  Lane *Road::GetNextLane(const double s, const LaneId lane_id) {
120 
121  auto upper = _lane_sections.upper_bound(s);
122 
123  while (upper != _lane_sections.end()) {
124  // check id
125  Lane *ptr = upper->second.GetLane(lane_id);
126  if (ptr != nullptr) {
127  return ptr;
128  }
129  ++upper;
130  }
131 
132  return nullptr;
133  }
134 
135  // get the lane on a section previous to 's'
136  Lane *Road::GetPrevLane(const double s, const LaneId lane_id) {
137 
138  auto lower = _lane_sections.lower_bound(s);
139  auto rlower = std::make_reverse_iterator(lower);
140 
141  while (rlower != _lane_sections.rend()) {
142  // check id
143  Lane *ptr = rlower->second.GetLane(lane_id);
144  if (ptr != nullptr) {
145  return ptr;
146  }
147  ++rlower;
148  }
149 
150  return nullptr;
151  }
152 
153  // get the start and end section with a lan id
155  auto it = _lane_sections.begin();
156  while (it != _lane_sections.end()) {
157  // check id
158  Lane *ptr = it->second.GetLane(id);
159  if (ptr != nullptr) {
160  return &(it->second);
161  }
162  ++it;
163  }
164  return nullptr;
165  }
166 
168  auto it = _lane_sections.rbegin();
169  while (it != _lane_sections.rend()) {
170  // check id
171  Lane *ptr = it->second.GetLane(id);
172  if (ptr != nullptr) {
173  return &(it->second);
174  }
175  ++it;
176  }
177  return nullptr;
178  }
179 
181  const auto clamped_s = geom::Math::Clamp(s, 0.0, _length);
182  const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
183 
184  const auto lane_offset = _info.GetInfo<element::RoadInfoLaneOffset>(clamped_s);
185  float offset = 0;
186  if(lane_offset){
187  offset = static_cast<float>(lane_offset->GetPolynomial().Evaluate(clamped_s));
188  }
189  // Apply road's lane offset record
190  element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
191  // Unreal's Y axis hack (the minus on the offset)
192  p.ApplyLateralOffset(-offset);
193 
194  // Apply road's elevation record
195  const auto elevation_info = GetElevationOn(s);
196  p.location.z = static_cast<float>(elevation_info.Evaluate(s));
197  p.pitch = elevation_info.Tangent(s);
198 
199  return p;
200  }
201 
203  const auto clamped_s = geom::Math::Clamp(s, 0.0, _length);
204  const auto geometry = _info.GetInfo<element::RoadInfoGeometry>(clamped_s);
205 
206  element::DirectedPoint p = geometry->GetGeometry().PosFromDist(clamped_s - geometry->GetDistance());
207 
208  // Apply road's elevation record
209  const auto elevation_info = GetElevationOn(s);
210  p.location.z = static_cast<float>(elevation_info.Evaluate(s));
211  p.pitch = elevation_info.Tangent(s);
212 
213  return p;
214  }
215 
216  const std::pair<double, double> Road::GetNearestPoint(const geom::Location &loc) const {
217  std::pair<double, double> last = { 0.0, std::numeric_limits<double>::max() };
218 
219  auto geom_info_list = _info.GetInfos<element::RoadInfoGeometry>();
220  decltype(geom_info_list)::iterator nearest_geom = geom_info_list.end();
221 
222  for (auto g = geom_info_list.begin(); g != geom_info_list.end(); ++g) {
223  DEBUG_ASSERT(*g != nullptr);
224  auto dist = (*g)->GetGeometry().DistanceTo(loc);
225  if (dist.second < last.second) {
226  last = dist;
227  nearest_geom = g;
228  }
229  }
230 
231  for (auto g = geom_info_list.begin();
232  g != geom_info_list.end() && g != nearest_geom;
233  ++g) {
234  DEBUG_ASSERT(*g != nullptr);
235  last.first += (*g)->GetGeometry().GetLength();
236  }
237 
238  return last;
239  }
240 
241  const std::pair<const Lane *, double> Road::GetNearestLane(
242  const double s,
243  const geom::Location &loc,
244  uint32_t lane_type) const {
245  using namespace carla::road::element;
246  std::map<LaneId, const Lane *> lanes(GetLanesAt(s));
247  // negative right lanes
248  auto right_lanes = MakeListView(
249  std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
250  // positive left lanes
251  auto left_lanes = MakeListView(
252  lanes.lower_bound(1), lanes.end());
253 
254  const DirectedPoint dp_lane_zero = GetDirectedPointIn(s);
255  std::pair<const Lane *, double> result =
256  std::make_pair(nullptr, std::numeric_limits<double>::max());
257 
258  DirectedPoint current_dp = dp_lane_zero;
259  for (const auto &lane : right_lanes) {
260  const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
261  const auto half_width = static_cast<float>(lane_width_info->GetPolynomial().Evaluate(s)) * 0.5f;
262 
263  current_dp.ApplyLateralOffset(half_width);
264  const auto current_dist = geom::Math::Distance(current_dp.location, loc);
265 
266  // if the current_dp is near to loc, we are in the right way
267  if (current_dist <= result.second) {
268  // only consider the lanes that match the type flag for result
269  // candidates
270  if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
271  result.first = &(*lane.second);
272  result.second = current_dist;
273  }
274  } else {
275  // elsewhere, we are be moving away
276  break;
277  }
278  current_dp.ApplyLateralOffset(half_width);
279  }
280 
281  current_dp = dp_lane_zero;
282  for (const auto &lane : left_lanes) {
283  const auto lane_width_info = lane.second->GetInfo<RoadInfoLaneWidth>(s);
284  const auto half_width = -static_cast<float>(lane_width_info->GetPolynomial().Evaluate(s)) * 0.5f;
285 
286  current_dp.ApplyLateralOffset(half_width);
287  const auto current_dist = geom::Math::Distance(current_dp.location, loc);
288 
289  // if the current_dp is near to loc, we are in the right way
290  if (current_dist <= result.second) {
291  // only consider the lanes that match the type flag for result
292  // candidates
293  if ((static_cast<uint32_t>(lane.second->GetType()) & lane_type) > 0) {
294  result.first = &(*lane.second);
295  result.second = current_dist;
296  }
297  } else {
298  // elsewhere, we are be moving away
299  break;
300  }
301  current_dp.ApplyLateralOffset(half_width);
302  }
303 
304  return result;
305  }
306 
307  std::map<LaneId, const Lane *> Road::GetLanesAt(const double s) const {
308  std::map<LaneId, const Lane *> map;
309  for (auto &&lane_section : GetLaneSectionsAt(s)) {
310  for (auto &&lane : lane_section.GetLanes()) {
311  map[lane.first] = &(lane.second);
312  }
313  }
314  return map;
315  }
316 
317 } // road
318 } // carla
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition: Math.h:74
std::string _name
Definition: Road.h:189
std::vector< Road * > _nexts
Definition: Road.h:205
uint32_t RoadId
Definition: RoadTypes.h:15
double _length
Definition: Road.h:191
Describes a Cubic Polynomial so: f(x) = a + bx + cx^2 + dx^3.
const std::pair< double, double > GetNearestPoint(const geom::Location &loc) const
Returns a pair containing:
Definition: Road.cpp:216
const std::pair< const Lane *, double > GetNearestLane(const double s, const geom::Location &loc, uint32_t type=static_cast< uint32_t >(Lane::LaneType::Any)) const
Returns a pointer to the nearest lane, given s relative to Road and a location.
Definition: Road.cpp:241
The lane offset record defines a lateral shift of the lane reference line(which is usually identical ...
RoadId GetSuccessor() const
Definition: Road.cpp:50
std::vector< Lane * > GetLanesByDistance(double s)
Get all lanes from all lane sections in a specific s.
Definition: Road.cpp:88
void throw_exception(const std::exception &e)
Definition: Carla.cpp:101
std::vector< Road * > GetNexts() const
Definition: Road.cpp:58
static T Clamp(T a, T min=T(0), T max=T(1))
Definition: Math.h:49
std::vector< Road * > _prevs
Definition: Road.h:207
bool IsJunction() const
Definition: Road.cpp:42
Lane * GetNextLane(const double s, const LaneId lane_id)
Definition: Road.cpp:119
const geom::CubicPolynomial & GetElevationOn(const double s) const
Definition: Road.cpp:66
Lane * GetPrevLane(const double s, const LaneId lane_id)
Definition: Road.cpp:136
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
LaneSection * GetEndSection(LaneId id)
Get the end section (from road coordinates s) given a lane id.
Definition: Road.cpp:167
Lane Width RecordEach lane within a road’scross section can be provided with severalwidth entries...
uint32_t SectionId
Definition: RoadTypes.h:21
JuncId _junction_id
Definition: Road.h:195
auto GetLaneSectionsAt(const double s)
Definition: Road.h:149
InformationSet _info
Definition: Road.h:203
int32_t JuncId
Definition: RoadTypes.h:17
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
const T * GetInfo(const double s) const
Returns single info given a type and a distance (s) from the start of the road.
RoadId GetId() const
Definition: Road.cpp:30
MapData * _map_data
Definition: Road.h:185
Lane & GetLaneByDistance(double s, LaneId lane_id)
Definition: Road.cpp:74
int32_t LaneId
Definition: RoadTypes.h:19
Lane & GetLaneById(SectionId section_id, LaneId lane_id)
Definition: Road.cpp:110
RoadId _successor
Definition: Road.h:199
LaneSectionMap _lane_sections
Definition: Road.h:197
LaneSection & GetLaneSectionById(SectionId id)
Definition: Road.h:163
RoadId _predecessor
Definition: Road.h:201
JuncId GetJunctionId() const
Definition: Road.cpp:46
RoadId GetPredecessor() const
Definition: Road.cpp:54
RoadId _id
Definition: Road.h:187
std::map< LaneId, const Lane * > GetLanesAt(const double s) const
Get all lanes at a given s.
Definition: Road.cpp:307
std::vector< const T * > GetInfos() const
Return all infos given a type from the start of the road.
std::map< LaneId, Lane > & GetLanes()
Definition: LaneSection.cpp:47
LaneSection * GetStartSection(LaneId id)
Get the start section (from road coordinates s) given a lane id.
Definition: Road.cpp:154
std::string GetName() const
Definition: Road.cpp:34
element::DirectedPoint GetDirectedPointInNoLaneOffset(const double s) const
Returns a directed point on the center of the road (lane 0), with the corresponding laneOffset and el...
Definition: Road.cpp:202
bool _is_junction
Definition: Road.h:193
const MapData * GetMap() const
Definition: Road.cpp:26
double GetLength() const
Definition: Road.cpp:38
static auto MakeListView(Iterator begin, Iterator end)
void ApplyLateralOffset(float lateral_offset)
Definition: Geometry.cpp:28
element::DirectedPoint GetDirectedPointIn(const double s) const
Returns a directed point on the center of the road (lane 0), with the corresponding laneOffset and el...
Definition: Road.cpp:180
std::vector< Road * > GetPrevs() const
Definition: Road.cpp:62