CARLA
client/Waypoint.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 
8 
9 #include "carla/client/Map.h"
10 #include "carla/client/Junction.h"
11 #include "carla/client/Landmark.h"
12 
13 #include <unordered_set>
14 
15 namespace carla {
16 namespace client {
17 
19  : _parent(std::move(parent)),
20  _waypoint(std::move(waypoint)),
21  _transform(_parent->GetMap().ComputeTransform(_waypoint)),
22  _mark_record(_parent->GetMap().GetMarkRecord(_waypoint)) {}
23 
24  Waypoint::~Waypoint() = default;
25 
27  return _parent->GetMap().GetJunctionId(_waypoint.road_id);
28  }
29 
30  bool Waypoint::IsJunction() const {
31  return _parent->GetMap().IsJunction(_waypoint.road_id);
32  }
33 
35  if (IsJunction()) {
36  return _parent->GetJunction(*this);
37  }
38  return nullptr;
39  }
40 
41  double Waypoint::GetLaneWidth() const {
42  return _parent->GetMap().GetLaneWidth(_waypoint);
43 
44  }
45 
47  return _parent->GetMap().GetLaneType(_waypoint);
48  }
49 
50  std::vector<SharedPtr<Waypoint>> Waypoint::GetNext(double distance) const {
51  auto waypoints = _parent->GetMap().GetNext(_waypoint, distance);
52  std::vector<SharedPtr<Waypoint>> result;
53  result.reserve(waypoints.size());
54  for (auto &waypoint : waypoints) {
55  result.emplace_back(SharedPtr<Waypoint>(new Waypoint(_parent, std::move(waypoint))));
56  }
57  return result;
58  }
59 
60  std::vector<SharedPtr<Waypoint>> Waypoint::GetPrevious(double distance) const {
61  auto waypoints = _parent->GetMap().GetPrevious(_waypoint, distance);
62  std::vector<SharedPtr<Waypoint>> result;
63  result.reserve(waypoints.size());
64  for (auto &waypoint : waypoints) {
65  result.emplace_back(SharedPtr<Waypoint>(new Waypoint(_parent, std::move(waypoint))));
66  }
67  return result;
68  }
69 
70  std::vector<SharedPtr<Waypoint>> Waypoint::GetNextUntilLaneEnd(double distance) const {
71  std::vector<SharedPtr<Waypoint>> result;
72  std::vector<SharedPtr<Waypoint>> next = GetNext(distance);
73 
74  while (next.size() == 1 && next.front()->GetRoadId() == GetRoadId()) {
75  result.emplace_back(next.front());
76  next = result.back()->GetNext(distance);
77  }
78  double current_s = GetDistance();
79  if(result.size()) {
80  current_s = result.back()->GetDistance();
81  }
82  double remaining_length;
83  double road_length = _parent->GetMap().GetLane(_waypoint).GetRoad()->GetLength();
84  if(_waypoint.lane_id < 0) {
85  remaining_length = road_length - current_s;
86  } else {
87  remaining_length = current_s;
88  }
89  remaining_length -= std::numeric_limits<double>::epsilon();
90  if(result.size()) {
91  result.emplace_back(result.back()->GetNext(remaining_length).front());
92  } else {
93  result.emplace_back(GetNext(remaining_length).front());
94  }
95 
96  return result;
97  }
98 
99  std::vector<SharedPtr<Waypoint>> Waypoint::GetPreviousUntilLaneStart(double distance) const {
100  std::vector<SharedPtr<Waypoint>> result;
101  std::vector<SharedPtr<Waypoint>> prev = GetPrevious(distance);
102 
103  while (prev.size() == 1 && prev.front()->GetRoadId() == GetRoadId()) {
104  result.emplace_back(prev.front());
105  prev = result.back()->GetPrevious(distance);
106  }
107 
108  double current_s = GetDistance();
109  if(result.size()) {
110  current_s = result.back()->GetDistance();
111  }
112 
113  double remaining_length;
114  double road_length = _parent->GetMap().GetLane(_waypoint).GetRoad()->GetLength();
115  if(_waypoint.lane_id < 0) {
116  remaining_length = road_length - current_s;
117  } else {
118  remaining_length = current_s;
119  }
120  remaining_length -= std::numeric_limits<double>::epsilon();
121  if(result.size()) {
122  result.emplace_back(result.back()->GetPrevious(remaining_length).front());
123  } else {
124  result.emplace_back(GetPrevious(remaining_length).front());
125  }
126 
127  return result;
128  }
129 
131  auto right_lane_waypoint =
132  _parent->GetMap().GetRight(_waypoint);
133  if (right_lane_waypoint.has_value()) {
134  return SharedPtr<Waypoint>(new Waypoint(_parent, std::move(*right_lane_waypoint)));
135  }
136  return nullptr;
137  }
138 
140  auto left_lane_waypoint =
141  _parent->GetMap().GetLeft(_waypoint);
142  if (left_lane_waypoint.has_value()) {
143  return SharedPtr<Waypoint>(new Waypoint(_parent, std::move(*left_lane_waypoint)));
144  }
145  return nullptr;
146  }
147 
148  boost::optional<road::element::LaneMarking> Waypoint::GetRightLaneMarking() const {
149  if (_mark_record.first != nullptr) {
151  }
152  return boost::optional<road::element::LaneMarking>{};
153  }
154 
155  boost::optional<road::element::LaneMarking> Waypoint::GetLeftLaneMarking() const {
156  if (_mark_record.second != nullptr) {
158  }
159  return boost::optional<road::element::LaneMarking>{};
160  }
161 
162  template <typename EnumT>
163  static EnumT operator&(EnumT lhs, EnumT rhs) {
164  return static_cast<EnumT>(
165  static_cast<typename std::underlying_type<EnumT>::type>(lhs) &
166  static_cast<typename std::underlying_type<EnumT>::type>(rhs));
167  }
168 
169  template <typename EnumT>
170  static EnumT operator|(EnumT lhs, EnumT rhs) {
171  return static_cast<EnumT>(
172  static_cast<typename std::underlying_type<EnumT>::type>(lhs) |
173  static_cast<typename std::underlying_type<EnumT>::type>(rhs));
174  }
175 
177  using lane_change_type = road::element::LaneMarking::LaneChange;
178 
179  const auto lane_change_right_info = _mark_record.first;
180  lane_change_type c_right;
181  if (lane_change_right_info != nullptr) {
182  const auto lane_change_right = lane_change_right_info->GetLaneChange();
183  c_right = static_cast<lane_change_type>(lane_change_right);
184  } else {
185  c_right = lane_change_type::Both;
186  }
187 
188  const auto lane_change_left_info = _mark_record.second;
189  lane_change_type c_left;
190  if (lane_change_left_info != nullptr) {
191  const auto lane_change_left = lane_change_left_info->GetLaneChange();
192  c_left = static_cast<lane_change_type>(lane_change_left);
193  } else {
194  c_left = lane_change_type::Both;
195  }
196 
197  if (_waypoint.lane_id > 0) {
198  // if road goes backward
199  if (c_right == lane_change_type::Right) {
200  c_right = lane_change_type::Left;
201  } else if (c_right == lane_change_type::Left) {
202  c_right = lane_change_type::Right;
203  }
204  }
205 
206  if (((_waypoint.lane_id > 0) ? _waypoint.lane_id - 1 : _waypoint.lane_id + 1) > 0) {
207  // if road goes backward
208  if (c_left == lane_change_type::Right) {
209  c_left = lane_change_type::Left;
210  } else if (c_left == lane_change_type::Left) {
211  c_left = lane_change_type::Right;
212  }
213  }
214 
215  return (c_right & lane_change_type::Right) | (c_left & lane_change_type::Left);
216  }
217 
218  std::vector<SharedPtr<Landmark>> Waypoint::GetAllLandmarksInDistance(
219  double distance, bool stop_at_junction) const {
220  std::vector<SharedPtr<Landmark>> result;
221  auto signals = _parent->GetMap().GetSignalsInDistance(
222  _waypoint, distance, stop_at_junction);
223  std::unordered_set<const road::element::RoadInfoSignal*> added_signals; // check for repeated signals
224  for(auto &signal_data : signals){
225  if(added_signals.count(signal_data.signal) > 0) {
226  continue;
227  }
228  added_signals.insert(signal_data.signal);
229  auto waypoint = SharedPtr<Waypoint>(new Waypoint(_parent, signal_data.waypoint));
230  result.emplace_back(
231  new Landmark(waypoint, _parent, signal_data.signal, signal_data.accumulated_s));
232  }
233  return result;
234  }
235 
236  std::vector<SharedPtr<Landmark>> Waypoint::GetLandmarksOfTypeInDistance(
237  double distance, std::string filter_type, bool stop_at_junction) const {
238  std::vector<SharedPtr<Landmark>> result;
239  std::unordered_set<const road::element::RoadInfoSignal*> added_signals; // check for repeated signals
240  auto signals = _parent->GetMap().GetSignalsInDistance(
241  _waypoint, distance, stop_at_junction);
242  for(auto &signal_data : signals){
243  if(signal_data.signal->GetSignal()->GetType() == filter_type) {
244  if(added_signals.count(signal_data.signal) > 0) {
245  continue;
246  }
247  auto waypoint = SharedPtr<Waypoint>(new Waypoint(_parent, signal_data.waypoint));
248  result.emplace_back(
249  new Landmark(waypoint, _parent, signal_data.signal, signal_data.accumulated_s));
250  }
251  }
252  return result;
253  }
254 
255 } // namespace client
256 } // namespace carla
std::vector< SharedPtr< Waypoint > > GetNext(double distance) const
static EnumT operator &(EnumT lhs, EnumT rhs)
std::vector< SharedPtr< Waypoint > > GetPreviousUntilLaneStart(double distance) const
Returns a list of waypoints separated by distance from the current waypoint to the start of the lane...
static EnumT operator|(EnumT lhs, EnumT rhs)
boost::shared_ptr< T > SharedPtr
Use this SharedPtr (boost::shared_ptr) to keep compatibility with boost::python, but it would be nice...
Definition: Memory.h:20
LaneChange
Can be used as flags.
Definition: LaneMarking.h:49
std::vector< SharedPtr< Landmark > > GetLandmarksOfTypeInDistance(double distance, std::string filter_type, bool stop_at_junction=false) const
Returns a list of landmarks from the current position to a certain distance Filters by specified type...
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
road::element::LaneMarking::LaneChange GetLaneChange() const
Class containing a reference to RoadInfoSignal.
Definition: Landmark.h:22
int32_t JuncId
Definition: RoadTypes.h:17
std::pair< const road::element::RoadInfoMarkRecord *, const road::element::RoadInfoMarkRecord * > _mark_record
std::vector< SharedPtr< Waypoint > > GetPrevious(double distance) const
road::JuncId GetJunctionId() const
road::Lane::LaneType GetType() const
LaneType
Can be used as flags.
Definition: Lane.h:29
Waypoint(SharedPtr< const Map > parent, road::element::Waypoint waypoint)
SharedPtr< Waypoint > GetLeft() const
SharedPtr< Junction > GetJunction() const
boost::optional< road::element::LaneMarking > GetRightLaneMarking() const
road::element::Waypoint _waypoint
boost::optional< road::element::LaneMarking > GetLeftLaneMarking() const
SharedPtr< Waypoint > GetRight() const
std::vector< SharedPtr< Landmark > > GetAllLandmarksInDistance(double distance, bool stop_at_junction=false) const
Returns a list of landmarks from the current position to a certain distance.
SharedPtr< const Map > _parent
std::vector< SharedPtr< Waypoint > > GetNextUntilLaneEnd(double distance) const
Returns a list of waypoints separated by distance from the current waypoint to the end of the lane...