61 lines
2.5 KiB
C
61 lines
2.5 KiB
C
|
// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma
|
||
|
|
// de Barcelona (UAB).
|
||
|
|
//
|
||
|
|
// This work is licensed under the terms of the MIT license.
|
||
|
|
// For a copy, see <https://opensource.org/licenses/MIT>.
|
||
|
|
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include "carla/client/Actor.h"
|
||
|
|
#include "carla/client/ActorList.h"
|
||
|
|
#include "carla/client/Vehicle.h"
|
||
|
|
#include "carla/client/World.h"
|
||
|
|
#include "carla/geom/Location.h"
|
||
|
|
#include "carla/road/RoadTypes.h"
|
||
|
|
#include "carla/rpc/ActorId.h"
|
||
|
|
|
||
|
|
#include "carla/trafficmanager/Constants.h"
|
||
|
|
#include "carla/trafficmanager/SimpleWaypoint.h"
|
||
|
|
#include "carla/trafficmanager/TrackTraffic.h"
|
||
|
|
|
||
|
|
namespace carla {
|
||
|
|
namespace traffic_manager {
|
||
|
|
|
||
|
|
namespace cc = carla::client;
|
||
|
|
namespace cg = carla::geom;
|
||
|
|
using Actor = carla::SharedPtr<cc::Actor>;
|
||
|
|
using ActorId = carla::ActorId;
|
||
|
|
using ActorIdSet = std::unordered_set<ActorId>;
|
||
|
|
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
|
||
|
|
using Buffer = std::deque<SimpleWaypointPtr>;
|
||
|
|
using GeoGridId = carla::road::JuncId;
|
||
|
|
using constants::Map::MAP_RESOLUTION;
|
||
|
|
using constants::Map::INV_MAP_RESOLUTION;
|
||
|
|
|
||
|
|
/// Returns the cross product (z component value) between the vehicle's
|
||
|
|
/// heading vector and the vector along the direction to the next
|
||
|
|
/// target waypoint on the horizon.
|
||
|
|
float DeviationCrossProduct(const cg::Location &reference_location,
|
||
|
|
const cg::Vector3D &heading_vector,
|
||
|
|
const cg::Location &target_location);
|
||
|
|
/// Returns the dot product between the vehicle's heading vector and
|
||
|
|
/// the vector along the direction to the next target waypoint on the horizon.
|
||
|
|
float DeviationDotProduct(const cg::Location &reference_location,
|
||
|
|
const cg::Vector3D &heading_vector,
|
||
|
|
const cg::Location &target_location);
|
||
|
|
|
||
|
|
// Function to add a waypoint to a path buffer and update waypoint tracking.
|
||
|
|
void PushWaypoint(ActorId actor_id, TrackTraffic& track_traffic,
|
||
|
|
Buffer& buffer, SimpleWaypointPtr& waypoint);
|
||
|
|
|
||
|
|
// Function to remove a waypoint from a path buffer and update waypoint tracking.
|
||
|
|
void PopWaypoint(ActorId actor_id, TrackTraffic& track_traffic,
|
||
|
|
Buffer& buffer, bool front_or_back=true);
|
||
|
|
|
||
|
|
/// Method to return the wayPoints from the waypoint Buffer by using target point distance
|
||
|
|
using TargetWPInfo = std::pair<SimpleWaypointPtr,uint64_t>;
|
||
|
|
TargetWPInfo GetTargetWaypoint(const Buffer& waypoint_buffer, const float& target_point_distance);
|
||
|
|
|
||
|
|
} // namespace traffic_manager
|
||
|
|
} // namespace carla
|