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  float dot_product = cg::Math::Dot(next_vector, heading_vector_flat);
34  dot_product = std::max(0.0f, std::min(dot_product, 1.0f));
35  return dot_product;
36 }
37 
38 void PushWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
39  Buffer &buffer, SimpleWaypointPtr &waypoint) {
40 
41  const uint64_t waypoint_id = waypoint->GetId();
42  buffer.push_back(waypoint);
43  track_traffic.UpdatePassingVehicle(waypoint_id, actor_id);
44 }
45 
46 void PopWaypoint(ActorId actor_id, TrackTraffic &track_traffic,
47  Buffer &buffer, bool front_or_back) {
48 
49  SimpleWaypointPtr removed_waypoint = front_or_back ? buffer.front() : buffer.back();
50  const uint64_t removed_waypoint_id = removed_waypoint->GetId();
51  if (front_or_back) {
52  buffer.pop_front();
53  } else {
54  buffer.pop_back();
55  }
56  track_traffic.RemovePassingVehicle(removed_waypoint_id, actor_id);
57 }
58 
59 TargetWPInfo GetTargetWaypoint(const Buffer &waypoint_buffer, const float &target_point_distance) {
60 
61  SimpleWaypointPtr target_waypoint = waypoint_buffer.front();
62  const SimpleWaypointPtr &buffer_front = waypoint_buffer.front();
63  uint64_t startPosn = static_cast<uint64_t>(std::fabs(target_point_distance * INV_MAP_RESOLUTION));
64  uint64_t index = 0;
65  /// Condition to determine forward or backward scanning of waypoint buffer.
66 
67  if (startPosn < waypoint_buffer.size()) {
68  bool mScanForward = false;
69  const float target_point_dist_power = target_point_distance * target_point_distance;
70  if (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power) {
71  mScanForward = true;
72  }
73 
74  if (mScanForward) {
75  for (uint64_t i = startPosn;
76  (i < waypoint_buffer.size()) && (buffer_front->DistanceSquared(target_waypoint) < target_point_dist_power);
77  ++i) {
78  target_waypoint = waypoint_buffer.at(i);
79  index = i;
80  }
81  } else {
82  for (uint64_t i = startPosn;
83  (buffer_front->DistanceSquared(target_waypoint) > target_point_dist_power);
84  --i) {
85  target_waypoint = waypoint_buffer.at(i);
86  index = i;
87  }
88  }
89  } else {
90  target_waypoint = waypoint_buffer.back();
91  index = waypoint_buffer.size() - 1;
92  }
93  return std::make_pair(target_waypoint, index);
94 }
95 
96 } // namespace traffic_manager
97 } // 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:133
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:95
Vector3D MakeSafeUnitVector(const float epsilon) const
Definition: geom/Vector3D.h:64
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