CARLA
SimpleWaypoint.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/geom/Math.h"
8 
10 
11 namespace carla {
12 namespace traffic_manager {
13 
14  using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
15 
17  waypoint = _waypoint;
18  next_left_waypoint = nullptr;
19  next_right_waypoint = nullptr;
20  }
22 
23  std::vector<SimpleWaypointPtr> SimpleWaypoint::GetNextWaypoint() const {
24  return next_waypoints;
25  }
26 
27  std::vector<SimpleWaypointPtr> SimpleWaypoint::GetPreviousWaypoint() const {
28  return previous_waypoints;
29  }
30 
32  return waypoint;
33  }
34 
35  uint64_t SimpleWaypoint::GetId() const {
36  return waypoint->GetId();
37  }
38 
40  return next_left_waypoint;
41  }
42 
44  return next_right_waypoint;
45  }
46 
48  return waypoint->GetTransform().location;
49  }
50 
52  return waypoint->GetTransform().rotation.GetForwardVector();
53  }
54 
55  uint64_t SimpleWaypoint::SetNextWaypoint(const std::vector<SimpleWaypointPtr> &waypoints) {
56  for (auto &simple_waypoint: waypoints) {
57  next_waypoints.push_back(simple_waypoint);
58  }
59  return static_cast<uint64_t>(waypoints.size());
60  }
61 
62  uint64_t SimpleWaypoint::SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &waypoints) {
63  for (auto &simple_waypoint: waypoints) {
64  previous_waypoints.push_back(simple_waypoint);
65  }
66  return static_cast<uint64_t>(waypoints.size());
67  }
68 
70 
71  const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
72  const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
73  if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) > 0.0f) {
74  next_left_waypoint = _waypoint;
75  }
76  }
77 
79 
80  const cg::Vector3D heading_vector = waypoint->GetTransform().GetForwardVector();
81  const cg::Vector3D relative_vector = GetLocation() - _waypoint->GetLocation();
82  if ((heading_vector.x * relative_vector.y - heading_vector.y * relative_vector.x) < 0.0f) {
83  next_right_waypoint = _waypoint;
84  }
85  }
86 
87  float SimpleWaypoint::Distance(const cg::Location &location) const {
88  return GetLocation().Distance(location);
89  }
90 
91  float SimpleWaypoint::Distance(const SimpleWaypointPtr &other) const {
92  return GetLocation().Distance(other->GetLocation());
93  }
94 
95  float SimpleWaypoint::DistanceSquared(const cg::Location &location) const {
96  return cg::Math::DistanceSquared(GetLocation(), location);
97  }
98 
100  return cg::Math::DistanceSquared(GetLocation(), other->GetLocation());
101  }
102 
104  return _is_junction;
105  }
106 
107  void SimpleWaypoint::SetIsJunction(bool value) {
108  _is_junction = value;
109  }
110 
112  return (next_waypoints.size() > 1);
113  }
114 
116  geodesic_grid_id = _geodesic_grid_id;
117  }
118 
120  GeoGridId grid_id;
121  if (waypoint->IsJunction()) {
122  grid_id = waypoint->GetJunctionId();
123  } else {
124  grid_id = geodesic_grid_id;
125  }
126  return grid_id;
127  }
128 
130  return waypoint->GetJunctionId();
131  }
132 
134  return waypoint->GetTransform();
135  }
136 
137 } // namespace traffic_manager
138 } // namespace carla
void SetIsJunction(bool value)
This method is used to set whether the waypoint belongs to a junction.
SimpleWaypointPtr next_right_waypoint
Pointer to right lane change waypoint.
uint64_t SetNextWaypoint(const std::vector< SimpleWaypointPtr > &next_waypoints)
This method is used to set the next waypoints.
SimpleWaypointPtr next_left_waypoint
Pointer to left lane change waypoint.
cg::Vector3D GetForwardVector() const
Returns the vector along the waypoint&#39;s direction.
void SetLeftWaypoint(SimpleWaypointPtr &waypoint)
This method is used to set the closest left waypoint for a lane change.
WaypointPtr waypoint
Pointer to Carla&#39;s waypoint object around which this class wraps around.
float DistanceSquared(const cg::Location &location) const
Calculates the square of the distance to given location.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
SimpleWaypointPtr GetRightWaypoint()
This method is used to get the closest right waypoint for a lane change.
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
carla::SharedPtr< cc::Waypoint > WaypointPtr
Definition: InMemoryMap.h:35
std::vector< SimpleWaypointPtr > GetPreviousWaypoint() const
Returns the list of previous waypoints.
bool CheckJunction() const
Returns true if the object&#39;s waypoint belongs to an intersection.
std::vector< SimpleWaypointPtr > previous_waypoints
List of pointers to previous connecting waypoints.
uint64_t GetId() const
Returns the unique id for the waypoint.
uint64_t SetPreviousWaypoint(const std::vector< SimpleWaypointPtr > &next_waypoints)
This method is used to set the previous waypoints.
bool CheckIntersection() const
Returns true if the object&#39;s waypoint belongs to an intersection (Doesn&#39;t use OpenDrive).
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
cg::Transform GetTransform() const
Return transform object for the current waypoint.
crd::JuncId GeoGridId
Definition: InMemoryMap.h:38
GeoGridId GetJunctionId() const
Method to retreive junction id of the waypoint.
std::vector< SimpleWaypointPtr > GetNextWaypoint() const
Returns the list of next waypoints.
cg::Location GetLocation() const
Returns the location object for this waypoint.
float Distance(const cg::Location &location) const
Calculates the distance from the object&#39;s waypoint to the passed location.
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
auto Distance(const Location &loc) const
Definition: geom/Location.h:36
SimpleWaypointPtr GetLeftWaypoint()
This method is used to get the closest left waypoint for a lane change.
std::vector< SimpleWaypointPtr > next_waypoints
List of pointers to next connecting waypoints.
void SetRightWaypoint(SimpleWaypointPtr &waypoint)
This method is used to set the closest right waypoint for a lane change.
void SetGeodesicGridId(GeoGridId _geodesic_grid_id)
Accessor methods for geodesic grid id.
WaypointPtr GetWaypoint() const
Returns a carla::shared_ptr to carla::waypoint.
GeoGridId geodesic_grid_id
Integer placing the waypoint into a geodesic grid.