CARLA
LaneCrossingCalculator.cpp
Go to the documentation of this file.
1 // Copyright (c) 2017 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 
9 
10 #include "carla/geom/Location.h"
11 #include "carla/road/Map.h"
12 
13 namespace carla {
14 namespace road {
15 namespace element {
16 
17  /// @todo Temporary flags to search lanes where we can find road marks.
18  /// This needs to be expanded searching in shoulders too, but since
19  /// shouders have a small width, they can cause problems while finding
20  /// the nearest center of a lane given a location that are in a road,
21  /// but very close to a shoulder.
22  static constexpr uint32_t FLAGS =
23  static_cast<uint32_t>(Lane::LaneType::Driving) |
24  static_cast<uint32_t>(Lane::LaneType::Bidirectional) |
25  static_cast<uint32_t>(Lane::LaneType::Biking) |
26  static_cast<uint32_t>(Lane::LaneType::Parking);
27 
28  /// Calculate the lane markings that need to be crossed from @a lane_id_origin
29  /// to @a lane_id_destination.
30  static std::vector<LaneMarking> CrossingAtSameSection(
31  const Map &map,
32  const Waypoint *w0,
33  const Waypoint *w1,
34  const bool w0_is_offroad,
35  const bool dest_is_at_right) {
36  auto w0_marks = map.GetMarkRecord(*w0);
37  auto w1_marks = map.GetMarkRecord(*w1);
38 
39  if (dest_is_at_right) {
40  if (w0_is_offroad) {
41  return { LaneMarking(*w1_marks.second) };
42  } else {
43  return { LaneMarking(*w0_marks.first) };
44  }
45  } else {
46  if (w0_is_offroad) {
47  return { LaneMarking(*w1_marks.first) };
48  } else {
49  return { LaneMarking(*w0_marks.second) };
50  }
51  }
52 
53  return {};
54  }
55 
56  static bool IsOffRoad(const Map &map, const geom::Location &location) {
57  return !map.GetWaypoint(location, FLAGS).has_value();
58  }
59 
60  std::vector<LaneMarking> LaneCrossingCalculator::Calculate(
61  const Map &map,
62  const geom::Location &origin,
63  const geom::Location &destination) {
64  auto w0 = map.GetClosestWaypointOnRoad(origin, FLAGS);
65  auto w1 = map.GetClosestWaypointOnRoad(destination, FLAGS);
66 
67  if (!w0.has_value() || !w1.has_value()) {
68  return {};
69  }
70 
71  if (w0->road_id != w1->road_id || w0->section_id != w1->section_id) {
72  /// @todo This case should also be handled.
73  return {};
74  }
75 
76  if (map.IsJunction(w0->road_id) || map.IsJunction(w1->road_id)) {
77  return {};
78  }
79 
80  const auto w0_is_offroad = IsOffRoad(map, origin);
81  const auto w1_is_offroad = IsOffRoad(map, destination);
82 
83  if (w0_is_offroad && w1_is_offroad) {
84  // outside the road
85  return {};
86  }
87 
88  if ((w0->lane_id == w1->lane_id) && !w0_is_offroad && !w1_is_offroad) {
89  // both at the same lane and inside the road
90  return {};
91  }
92 
93  const auto transform = map.ComputeTransform(*w0);
94  geom::Vector3D orig_vec = transform.GetForwardVector();
95  geom::Vector3D dest_vec = (destination - origin).MakeSafeUnitVector(2 * std::numeric_limits<float>::epsilon());
96 
97  // cross product
98  const auto dest_is_at_right =
99  (-orig_vec.x * dest_vec.y + orig_vec.y * dest_vec.x) < 0;
100 
101  return CrossingAtSameSection(
102  map,
103  &*w0,
104  &*w1,
105  w0_is_offroad,
106  dest_is_at_right);
107  }
108 
109 } // namespace element
110 } // namespace road
111 } // namespace carla
boost::optional< element::Waypoint > GetWaypoint(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
Definition: road/Map.cpp:203
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
static bool IsOffRoad(const Map &map, const geom::Location &location)
static constexpr uint32_t FLAGS
boost::optional< element::Waypoint > GetClosestWaypointOnRoad(const geom::Location &location, int32_t lane_type=static_cast< int32_t >(Lane::LaneType::Driving)) const
======================================================================== – Geometry ----------------...
Definition: road/Map.cpp:156
geom::Transform ComputeTransform(Waypoint waypoint) const
Definition: road/Map.cpp:264
std::pair< const element::RoadInfoMarkRecord *, const element::RoadInfoMarkRecord * > GetMarkRecord(Waypoint waypoint) const
Definition: road/Map.cpp:298
bool IsJunction(RoadId road_id) const
Definition: road/Map.cpp:293
static std::vector< LaneMarking > Calculate(const Map &map, const geom::Location &origin, const geom::Location &destination)
static std::vector< LaneMarking > CrossingAtSameSection(const Map &map, const Waypoint *w0, const Waypoint *w1, const bool w0_is_offroad, const bool dest_is_at_right)
Calculate the lane markings that need to be crossed from lane_id_origin to lane_id_destination.