141 lines
4.6 KiB
C++
141 lines
4.6 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 <memory.h>
|
|
|
|
#include "carla/client/Waypoint.h"
|
|
#include "carla/geom/Location.h"
|
|
#include "carla/geom/Transform.h"
|
|
#include "carla/geom/Vector3D.h"
|
|
#include "carla/Memory.h"
|
|
#include "carla/road/RoadTypes.h"
|
|
|
|
namespace carla {
|
|
namespace traffic_manager {
|
|
|
|
namespace cc = carla::client;
|
|
namespace cg = carla::geom;
|
|
using WaypointPtr = carla::SharedPtr<cc::Waypoint>;
|
|
using GeoGridId = carla::road::JuncId;
|
|
enum class RoadOption : uint8_t {
|
|
Void = 0,
|
|
Left = 1,
|
|
Right = 2,
|
|
Straight = 3,
|
|
LaneFollow = 4,
|
|
ChangeLaneLeft = 5,
|
|
ChangeLaneRight = 6,
|
|
RoadEnd = 7
|
|
};
|
|
|
|
/// This is a simple wrapper class on Carla's waypoint object.
|
|
/// The class is used to represent discrete samples of the world map.
|
|
class SimpleWaypoint {
|
|
|
|
using SimpleWaypointPtr = std::shared_ptr<SimpleWaypoint>;
|
|
|
|
private:
|
|
|
|
/// Pointer to Carla's waypoint object around which this class wraps around.
|
|
WaypointPtr waypoint;
|
|
/// List of pointers to next connecting waypoints.
|
|
std::vector<SimpleWaypointPtr> next_waypoints;
|
|
/// List of pointers to previous connecting waypoints.
|
|
std::vector<SimpleWaypointPtr> previous_waypoints;
|
|
/// Pointer to left lane change waypoint.
|
|
SimpleWaypointPtr next_left_waypoint;
|
|
/// Pointer to right lane change waypoint.
|
|
SimpleWaypointPtr next_right_waypoint;
|
|
/// RoadOption for the actual waypoint.
|
|
RoadOption road_option = RoadOption::Void;
|
|
/// Integer placing the waypoint into a geodesic grid.
|
|
GeoGridId geodesic_grid_id = 0;
|
|
// Boolean to hold if the waypoint belongs to a junction
|
|
bool _is_junction = false;
|
|
|
|
public:
|
|
|
|
SimpleWaypoint(WaypointPtr _waypoint);
|
|
~SimpleWaypoint();
|
|
|
|
/// Returns the location object for this waypoint.
|
|
cg::Location GetLocation() const;
|
|
|
|
/// Returns a carla::shared_ptr to carla::waypoint.
|
|
WaypointPtr GetWaypoint() const;
|
|
|
|
/// Returns the list of next waypoints.
|
|
std::vector<SimpleWaypointPtr> GetNextWaypoint() const;
|
|
|
|
/// Returns the list of previous waypoints.
|
|
std::vector<SimpleWaypointPtr> GetPreviousWaypoint() const;
|
|
|
|
/// Returns the vector along the waypoint's direction.
|
|
cg::Vector3D GetForwardVector() const;
|
|
|
|
/// Returns the unique id for the waypoint.
|
|
uint64_t GetId() const;
|
|
|
|
/// This method is used to set the next waypoints.
|
|
uint64_t SetNextWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
|
|
|
|
/// This method is used to set the previous waypoints.
|
|
uint64_t SetPreviousWaypoint(const std::vector<SimpleWaypointPtr> &next_waypoints);
|
|
|
|
/// This method is used to set the closest left waypoint for a lane change.
|
|
void SetLeftWaypoint(SimpleWaypointPtr &waypoint);
|
|
|
|
/// This method is used to set the closest right waypoint for a lane change.
|
|
void SetRightWaypoint(SimpleWaypointPtr &waypoint);
|
|
|
|
/// This method is used to get the closest left waypoint for a lane change.
|
|
SimpleWaypointPtr GetLeftWaypoint();
|
|
|
|
/// This method is used to get the closest right waypoint for a lane change.
|
|
SimpleWaypointPtr GetRightWaypoint();
|
|
|
|
/// Accessor methods for geodesic grid id.
|
|
void SetGeodesicGridId(GeoGridId _geodesic_grid_id);
|
|
GeoGridId GetGeodesicGridId();
|
|
|
|
/// Method to retreive junction id of the waypoint.
|
|
GeoGridId GetJunctionId() const;
|
|
|
|
/// Calculates the distance from the object's waypoint to the passed
|
|
/// location.
|
|
float Distance(const cg::Location &location) const;
|
|
|
|
/// Calculates the distance the other SimpleWaypoint object.
|
|
float Distance(const SimpleWaypointPtr &other) const;
|
|
|
|
/// Calculates the square of the distance to given location.
|
|
float DistanceSquared(const cg::Location &location) const;
|
|
|
|
/// Calculates the square of the distance to other waypoints.
|
|
float DistanceSquared(const SimpleWaypointPtr &other) const;
|
|
|
|
/// Returns true if the object's waypoint belongs to an intersection.
|
|
bool CheckJunction() const;
|
|
|
|
/// This method is used to set whether the waypoint belongs to a junction.
|
|
void SetIsJunction(bool value);
|
|
|
|
/// Returns true if the object's waypoint belongs to an intersection (Doesn't use OpenDrive).
|
|
bool CheckIntersection() const;
|
|
|
|
/// Return transform object for the current waypoint.
|
|
cg::Transform GetTransform() const;
|
|
|
|
// Accessor methods for road option.
|
|
void SetRoadOption(RoadOption _road_option);
|
|
RoadOption GetRoadOption();
|
|
};
|
|
|
|
} // namespace traffic_manager
|
|
} // namespace carla
|