CARLA
LocalizationUtils.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 
9 
10 
11 namespace carla {
12 namespace traffic_manager {
13 
15 
16 float DeviationCrossProduct(const cg::Location &reference_location,
17  const cg::Vector3D &heading_vector,
18  const cg::Location &target_location) {
19  cg::Vector3D next_vector = target_location - reference_location;
20  next_vector = next_vector.MakeSafeUnitVector(EPSILON);
21  const float cross_z = heading_vector.x * next_vector.y - heading_vector.y * next_vector.x;
22  return cross_z;
23 }
24 
25 float DeviationDotProduct(const cg::Location &reference_location,
26  const cg::Vector3D &heading_vector,
27  const cg::Location &target_location) {
28  cg::Vector3D next_vector = target_location - reference_location;
29  next_vector.z = 0.0f;
30  next_vector = next_vector.MakeSafeUnitVector(EPSILON);
31  cg::Vector3D heading_vector_flat(heading_vector.x, heading_vector.y, 0);
32  heading_vector_flat = heading_vector_flat.MakeSafeUnitVector(EPSILON);
33  const float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
34  return dot_product;
35 }
36 
37 void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
38  Buffer &buffer, SimpleWaypointPtr &waypoint) {
39 
40  const uint64_t waypoint_id = waypoint->GetId();
41  buffer.push_back(waypoint);
42  track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
43 }
44 
45 void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
46  Buffer &buffer, bool front_or_back) {
47 
48  SimpleWaypointPtr removed_waypoint = front_or_back ? buffer.front() : buffer.back();
49  const uint64_t removed_waypoint_id = removed_waypoint->GetId();
50  if (front_or_back) {
51  buffer.pop_front();
52  } else {
53  buffer.pop_back();
54  }
55  track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);
56 }
57 
58 TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance) {
59 
60  SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
61  const SimpleWaypointPtr &buffer_front = waypoint_buffer.front();
62  uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance * INV_MAP_RESOLUTION));
63  uint64_t index = 0;
64  /// Condition to determine forward or backward scanning of WayPoint Buffer.
65 
66  if (startPosn < waypoint_buffer.size()) {
67  bool mScanForward = false;
68  const float target_point_dist_power = target_point_distance * target_point_distance;
69  if (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power) {
70  mScanForward = true;
71  }
72 
73  if (mScanForward) {
74  for (uint64_t i = startPosn;
75  (i < waypoint_buffer.size()) && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
76  ++i) {
77  target_waypoint = waypoint_buffer.at(i);
78  index = i;
79  }
80  } else {
81  for (uint64_t i = startPosn;
82  (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power);
83  --i) {
84  target_waypoint = waypoint_buffer.at(i);
85  index = i;
86  }
87  }
88  } else {
89  target_waypoint = waypoint_buffer.back();
90  index = waypoint_buffer.size() - 1;
91  }
92  return std::make_pair(target_waypoint, index);
93 }
94 
95 } // namespace traffic_manager
96 } // namespace carla
TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance)
void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic, Buffer &buffer, bool front_or_back)
float DeviationDotProduct(const cg::Location &reference_location, const cg::Vector3D &heading_vector, const cg::Location &target_location)
Returns the dot product between the vehicle&#39;s heading vector and the vector along the direction to th...
void UpdatePassingVehicle(uint64_t waypoint_id, ActorId actor_id)
Methods to update, remove and retrieve vehicles passing through a waypoint.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic, Buffer &buffer, SimpleWaypointPtr &waypoint)
std::deque< std::shared_ptr< SimpleWaypoint > > Buffer
void RemovePassingVehicle(uint64_t waypoint_id, ActorId actor_id)
carla::ActorId ActorId
float DeviationCrossProduct(const cg::Location &reference_location, const cg::Vector3D &heading_vector, const cg::Location &target_location)
Returns the cross product (z component value) between the vehicle&#39;s heading vector and the vector alo...
static auto Dot(const Vector3D &a, const Vector3D &b)
Definition: Math.h:58
std::shared_ptr< SimpleWaypoint > SimpleWaypointPtr
std::pair< SimpleWaypointPtr, uint64_t > TargetWPInfo
Method to return the wayPoints from the waypoint Buffer by using target point distance.
static const float INV_MAP_RESOLUTION
Definition: Constants.h:98
Vector3D MakeSafeUnitVector(const float epsilon) const
Definition: geom/Vector3D.h:60
static constexpr double EPSILON
We use this epsilon to shift the waypoints away from the edges of the lane sections to avoid floating...
Definition: road/Map.cpp:31