CARLA
Lane.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/road/Lane.h"
8 
9 #include <limits>
10 
11 #include "carla/Debug.h"
12 #include "carla/geom/Math.h"
18 #include "carla/road/LaneSection.h"
19 #include "carla/road/MapData.h"
20 #include "carla/road/Road.h"
21 
22 namespace carla {
23 namespace road {
24 
26  return _lane_section;
27  }
28 
29  Road *Lane::GetRoad() const {
30  DEBUG_ASSERT(_lane_section != nullptr);
31  return _lane_section->GetRoad();
32  }
33 
34  LaneId Lane::GetId() const {
35  return _id;
36  }
37 
39  return _type;
40  }
41 
42  bool Lane::GetLevel() const {
43  return _level;
44  }
45 
46  double Lane::GetDistance() const {
47  DEBUG_ASSERT(_lane_section != nullptr);
48  return _lane_section->GetDistance();
49  }
50 
51  double Lane::GetLength() const {
52  const auto *road = GetRoad();
53  DEBUG_ASSERT(road != nullptr);
54  const auto s = GetDistance();
55  return road->UpperBound(s) - s;
56  }
57 
58  double Lane::GetWidth(const double s) const {
59  RELEASE_ASSERT(s <= GetRoad()->GetLength());
60  const auto width_info = GetInfo<element::RoadInfoLaneWidth>(s);
61  RELEASE_ASSERT(width_info != nullptr);
62  return width_info->GetPolynomial().Evaluate(s);
63  }
64 
65  bool Lane::IsStraight() const {
66  Road *road = GetRoad();
67  RELEASE_ASSERT(road != nullptr);
68  auto *geometry = road->GetInfo<element::RoadInfoGeometry>(GetDistance());
69  DEBUG_ASSERT(geometry != nullptr);
70  auto geometry_type = geometry->GetGeometry().GetType();
71  if (geometry_type != element::GeometryType::LINE) {
72  return false;
73  }
74  if(GetDistance() < geometry->GetDistance() ||
75  GetDistance() + GetLength() >
76  geometry->GetDistance() + geometry->GetGeometry().GetLength()) {
77  return false;
78  }
79  auto lane_offsets = GetInfos<element::RoadInfoLaneOffset>();
80  for (auto *lane_offset : lane_offsets) {
81  if (std::abs(lane_offset->GetPolynomial().GetC()) > 0 ||
82  std::abs(lane_offset->GetPolynomial().GetD()) > 0) {
83  return false;
84  }
85  }
86  auto elevations = road->GetInfos<element::RoadInfoElevation>();
87  for (auto *elevation : elevations) {
88  if (std::abs(elevation->GetPolynomial().GetC()) > 0 ||
89  std::abs(elevation->GetPolynomial().GetD()) > 0) {
90  return false;
91  }
92  }
93  return true;
94  }
95 
96  /// Returns a pair containing first = width, second = tangent,
97  /// for an specific Lane given an s and a iterator over lanes
98  template <typename T>
99  static std::pair<double, double> ComputeTotalLaneWidth(
100  const T container,
101  const double s,
102  const LaneId lane_id) {
103 
104  // lane_id can't be 0
105  RELEASE_ASSERT(lane_id != 0);
106 
107  const bool negative_lane_id = lane_id < 0;
108  double dist = 0.0;
109  double tangent = 0.0;
110  for (const auto &lane : container) {
111  auto info = lane.second.template GetInfo<element::RoadInfoLaneWidth>(s);
112  RELEASE_ASSERT(info != nullptr);
113  const auto current_polynomial = info->GetPolynomial();
114  auto current_dist = current_polynomial.Evaluate(s);
115  auto current_tang = current_polynomial.Tangent(s);
116  if (lane.first != lane_id) {
117  dist += negative_lane_id ? current_dist : -current_dist;
118  tangent += negative_lane_id ? current_tang : -current_tang;
119  } else {
120  current_dist *= 0.5;
121  dist += negative_lane_id ? current_dist : -current_dist;
122  tangent += (negative_lane_id ? current_tang : -current_tang) * 0.5;
123  break;
124  }
125  }
126  return std::make_pair(dist, tangent);
127  }
128 
129  geom::Transform Lane::ComputeTransform(const double s) const {
130  const Road *road = GetRoad();
131  DEBUG_ASSERT(road != nullptr);
132 
133  // must s be smaller (or eq) than road length and bigger (or eq) than 0?
134  RELEASE_ASSERT(s <= road->GetLength());
135  RELEASE_ASSERT(s >= 0.0);
136 
137  const auto *lane_section = GetLaneSection();
138  DEBUG_ASSERT(lane_section != nullptr);
139  const std::map<LaneId, Lane> &lanes = lane_section->GetLanes();
140 
141  // check that lane_id exists on the current s
142  RELEASE_ASSERT(!lanes.empty());
143  RELEASE_ASSERT(GetId() >= lanes.begin()->first);
144  RELEASE_ASSERT(GetId() <= lanes.rbegin()->first);
145 
146  // These will accumulate the lateral offset (t) and lane heading of all
147  // the lanes in between the current lane and lane 0, where the main road
148  // geometry is described
149  float lane_t_offset = 0.0f;
150  float lane_tangent = 0.0f;
151 
152  if (GetId() < 0) {
153  // right lane
154  const auto side_lanes = MakeListView(
155  std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
156  const auto computed_width =
157  ComputeTotalLaneWidth(side_lanes, s, GetId());
158  lane_t_offset = static_cast<float>(computed_width.first);
159  lane_tangent = static_cast<float>(computed_width.second);
160  } else if (GetId() > 0) {
161  // left lane
162  const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end());
163  const auto computed_width =
164  ComputeTotalLaneWidth(side_lanes, s, GetId());
165  lane_t_offset = static_cast<float>(computed_width.first);
166  lane_tangent = static_cast<float>(computed_width.second);
167  }
168 
169  // Compute the tangent of the road's (lane 0) "laneOffset" on the current s
170  const auto lane_offset_info = road->GetInfo<element::RoadInfoLaneOffset>(s);
171  const auto lane_offset_tangent =
172  static_cast<float>(lane_offset_info->GetPolynomial().Tangent(s));
173 
174  // Update the road tangent with the "laneOffset" information at current s
175  lane_tangent -= lane_offset_tangent;
176 
177  // Get a directed point on the center of the current lane given an s
179 
180  // Transform from the center of the road to the center of the lane
181  dp.ApplyLateralOffset(lane_t_offset);
182 
183  // Update the lane tangent with the road "laneOffset" at current s
184  dp.tangent -= lane_tangent;
185 
186  // Unreal's Y axis hack
187  dp.location.y *= -1;
188  dp.tangent *= -1;
189 
190  geom::Rotation rot(
191  geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
192  geom::Math::ToDegrees(static_cast<float>(dp.tangent)),
193  0.0f);
194 
195  // Fix the direction of the possitive lanes
196  if (GetId() > 0) {
197  rot.yaw += 180.0f;
198  rot.pitch = 360.0f - rot.pitch;
199  }
200 
201  return geom::Transform(dp.location, rot);
202  }
203 
204  std::pair<geom::Vector3D, geom::Vector3D> Lane::GetCornerPositions(
205  const double s, const float extra_width) const {
206  const Road *road = GetRoad();
207  DEBUG_ASSERT(road != nullptr);
208 
209  const auto *lane_section = GetLaneSection();
210  DEBUG_ASSERT(lane_section != nullptr);
211  const std::map<LaneId, Lane> &lanes = lane_section->GetLanes();
212 
213  // check that lane_id exists on the current s
214  RELEASE_ASSERT(!lanes.empty());
215  RELEASE_ASSERT(GetId() >= lanes.begin()->first);
216  RELEASE_ASSERT(GetId() <= lanes.rbegin()->first);
217 
218  float lane_t_offset = 0.0f;
219 
220  if (GetId() < 0) {
221  // right lane
222  const auto side_lanes = MakeListView(
223  std::make_reverse_iterator(lanes.lower_bound(0)), lanes.rend());
224  const auto computed_width =
225  ComputeTotalLaneWidth(side_lanes, s, GetId());
226  lane_t_offset = static_cast<float>(computed_width.first);
227  } else if (GetId() > 0) {
228  // left lane
229  const auto side_lanes = MakeListView(lanes.lower_bound(1), lanes.end());
230  const auto computed_width =
231  ComputeTotalLaneWidth(side_lanes, s, GetId());
232  lane_t_offset = static_cast<float>(computed_width.first);
233  }
234 
235  float lane_width = static_cast<float>(GetWidth(s)) / 2.0f;
236  if (extra_width != 0.f && road->IsJunction() && GetType() == Lane::LaneType::Driving) {
237  lane_width += extra_width;
238  }
239 
240  // Get two points on the center of the road on given s
241  element::DirectedPoint dp_r, dp_l;
242  dp_r = dp_l = road->GetDirectedPointIn(s);
243 
244  // Transform from the center of the road to each of lane corners
245  dp_r.ApplyLateralOffset(lane_t_offset + lane_width);
246  dp_l.ApplyLateralOffset(lane_t_offset - lane_width);
247 
248  // Unreal's Y axis hack
249  dp_r.location.y *= -1;
250  dp_l.location.y *= -1;
251 
252  // Apply an offset to the Sidewalks
253  if (GetType() == LaneType::Sidewalk) {
254  // RoadRunner doesn't export it right now and as a workarround where 15.24 cm
255  // is the exact height that match with most of the RoadRunner sidewalks
256  dp_r.location.z += 0.1524f;
257  dp_l.location.z += 0.1524f;
258  /// @TODO: use the OpenDRIVE 5.3.7.2.1.1.9 Lane Height Record
259  }
260 
261  return std::make_pair(dp_r.location, dp_l.location);
262  }
263 
264 } // road
265 } // carla
Road * GetRoad() const
Definition: LaneSection.cpp:23
LaneType GetType() const
Definition: Lane.cpp:38
std::vector< const T * > GetInfos() const
Definition: Road.h:118
double GetDistance() const
Definition: LaneSection.cpp:13
LaneType _type
Definition: Lane.h:132
The lane offset record defines a lateral shift of the lane reference line(which is usually identical ...
Road * GetRoad() const
Definition: Lane.cpp:29
bool IsJunction() const
Definition: Road.cpp:42
double GetDistance() const
Definition: Lane.cpp:46
double GetLength() const
Definition: Lane.cpp:51
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
double GetWidth(const double s) const
Returns the total lane width given a s.
Definition: Lane.cpp:58
const T * GetInfo(const double s) const
Definition: Road.h:113
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
LaneSection * _lane_section
Definition: Lane.h:126
#define RELEASE_ASSERT(pred)
Definition: Debug.h:84
LaneType
Can be used as flags.
Definition: Lane.h:29
int32_t LaneId
Definition: RoadTypes.h:19
LaneId _id
Definition: Lane.h:128
bool IsStraight() const
Checks whether the geometry is straight or not.
Definition: Lane.cpp:65
static std::pair< double, double > ComputeTotalLaneWidth(const T container, const double s, const LaneId lane_id)
Returns a pair containing first = width, second = tangent, for an specific Lane given an s and a iter...
Definition: Lane.cpp:99
geom::Transform ComputeTransform(const double s) const
Definition: Lane.cpp:129
static constexpr T ToDegrees(T rad)
Definition: Math.h:37
static auto MakeListView(Iterator begin, Iterator end)
bool GetLevel() const
Definition: Lane.cpp:42
void ApplyLateralOffset(float lateral_offset)
Definition: Geometry.cpp:28
std::pair< geom::Vector3D, geom::Vector3D > GetCornerPositions(const double s, const float extra_width=0.f) const
Computes the location of the edges given a s.
Definition: Lane.cpp:204
geom::Transform Transform
Definition: rpc/Transform.h:16
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
LaneId GetId() const
Definition: Lane.cpp:34
const LaneSection * GetLaneSection() const
Definition: Lane.cpp:25