C++ port of agents.navigation.global_route_planner.GlobalRoutePlanner. More...

#include <GlobalRoutePlanner.h>

Classes
struct	Edge

struct	Node

struct	TopologySegment

Public Types
using	RouteItem = std::pair< SharedPtr< client::Waypoint >, RoadOption >

Public Member Functions
	GlobalRoutePlanner (SharedPtr< client::Map > map, double sampling_resolution)

std::vector< RouteItem >	TraceRoute (const geom::Location &origin, const geom::Location &destination)
	Returns the list of (waypoint, RoadOption) pairs from `origin` to `destination`.

Private Types
using	NodeId = int

using	XYZKey = std::tuple< double, double, double >

Private Member Functions
Edge &	AddEdge (NodeId n1, NodeId n2)

void	BuildGraph ()

void	BuildTopology ()

double	DistanceHeuristic (NodeId a, NodeId b) const

Edge *	FindEdge (NodeId n1, NodeId n2)

const Edge *	FindEdge (NodeId n1, NodeId n2) const

void	FindLooseEnds ()

NodeId	GetOrCreateNode (const XYZKey &key)

void	LaneChangeLink ()

std::optional< std::pair< NodeId, NodeId > >	Localize (const geom::Location &loc) const

std::vector< NodeId >	PathSearch (const geom::Location &origin, const geom::Location &destination)

std::pair< std::optional< NodeId >, const Edge * >	SuccessiveLastIntersectionEdge (std::size_t index, const std::vector< NodeId > &route) const

RoadOption	TurnDecision (std::size_t index, const std::vector< NodeId > &route)

Static Private Member Functions
static std::size_t	FindClosestInList (const SharedPtr< client::Waypoint > &current, const std::vector< SharedPtr< client::Waypoint > > &list)

Private Attributes
std::map< std::pair< NodeId, NodeId >, Edge >	_edges

std::map< XYZKey, NodeId >	_id_map

NodeId	_intersection_end_node = -1

SharedPtr< client::Map >	_map

std::unordered_map< NodeId, Node >	_nodes

RoadOption	_previous_decision = RoadOption::Void

std::map< uint32_t, std::map< uint32_t, std::map< int32_t, std::pair< NodeId, NodeId > > > >	_road_id_to_edge

double	_sampling_resolution

std::vector< TopologySegment >	_topology

Detailed Description

C++ port of agents.navigation.global_route_planner.GlobalRoutePlanner.

Builds a directed graph from carla::client::Map::GetTopology() and runs A* search (euclidean heuristic, edge weight = length) to produce a (waypoint, RoadOption) plan between two world locations.

Definition at line 36 of file GlobalRoutePlanner.h.

Member Typedef Documentation

◆ NodeId

using carla::agents::navigation::GlobalRoutePlanner::NodeId = int

private

Definition at line 50 of file GlobalRoutePlanner.h.

◆ RouteItem

using carla::agents::navigation::GlobalRoutePlanner::RouteItem = std::pair<SharedPtr<client::Waypoint>, RoadOption>

Definition at line 39 of file GlobalRoutePlanner.h.

◆ XYZKey

using carla::agents::navigation::GlobalRoutePlanner::XYZKey = std::tuple<double, double, double>

private

Definition at line 51 of file GlobalRoutePlanner.h.

Constructor & Destructor Documentation

◆ GlobalRoutePlanner()

carla::agents::navigation::GlobalRoutePlanner::GlobalRoutePlanner	(	SharedPtr< client::Map >	map,
		double	sampling_resolution
	)

Definition at line 56 of file GlobalRoutePlanner.cpp.

References BuildGraph(), BuildTopology(), FindLooseEnds(), and LaneChangeLink().

Member Function Documentation

◆ AddEdge()

GlobalRoutePlanner::Edge & carla::agents::navigation::GlobalRoutePlanner::AddEdge	(	NodeId	n1,
		NodeId	n2
	)

private

Definition at line 82 of file GlobalRoutePlanner.cpp.

References _edges, _nodes, node, and carla::agents::navigation::GlobalRoutePlanner::Edge::src.

Referenced by BuildGraph(), FindLooseEnds(), and LaneChangeLink().

◆ BuildGraph()

void carla::agents::navigation::GlobalRoutePlanner::BuildGraph ( )

private

Definition at line 167 of file GlobalRoutePlanner.cpp.

Referenced by GlobalRoutePlanner().

◆ BuildTopology()

void carla::agents::navigation::GlobalRoutePlanner::BuildTopology ( )

private

Definition at line 110 of file GlobalRoutePlanner.cpp.

References _map, _sampling_resolution, _topology, carla::agents::navigation::GlobalRoutePlanner::TopologySegment::entry, carla::agents::navigation::GlobalRoutePlanner::TopologySegment::entry_xyz, carla::agents::navigation::GlobalRoutePlanner::TopologySegment::exit, carla::agents::navigation::GlobalRoutePlanner::TopologySegment::exit_xyz, and carla::agents::navigation::GlobalRoutePlanner::TopologySegment::path.

Referenced by GlobalRoutePlanner().

◆ DistanceHeuristic()

double carla::agents::navigation::GlobalRoutePlanner::DistanceHeuristic	(	NodeId	a,
		NodeId	b
	)		const

private

Definition at line 369 of file GlobalRoutePlanner.cpp.

References _nodes.

Referenced by PathSearch().

◆ FindClosestInList()

std::size_t carla::agents::navigation::GlobalRoutePlanner::FindClosestInList	(	const SharedPtr< client::Waypoint > &	current,
		const std::vector< SharedPtr< client::Waypoint > > &	list
	)

staticprivate

Definition at line 590 of file GlobalRoutePlanner.cpp.

Referenced by TraceRoute().

◆ FindEdge() [1/2]

GlobalRoutePlanner::Edge * carla::agents::navigation::GlobalRoutePlanner::FindEdge	(	NodeId	n1,
		NodeId	n2
	)

private

Definition at line 102 of file GlobalRoutePlanner.cpp.

References _edges.

◆ FindEdge() [2/2]

const GlobalRoutePlanner::Edge * carla::agents::navigation::GlobalRoutePlanner::FindEdge	(	NodeId	n1,
		NodeId	n2
	)		const

private

Definition at line 96 of file GlobalRoutePlanner.cpp.

References _edges.

Referenced by PathSearch(), SuccessiveLastIntersectionEdge(), TraceRoute(), and TurnDecision().

◆ FindLooseEnds()

void carla::agents::navigation::GlobalRoutePlanner::FindLooseEnds ( )

private

Definition at line 208 of file GlobalRoutePlanner.cpp.

Referenced by GlobalRoutePlanner().

◆ GetOrCreateNode()

GlobalRoutePlanner::NodeId carla::agents::navigation::GlobalRoutePlanner::GetOrCreateNode ( const XYZKey & key )

private

Definition at line 69 of file GlobalRoutePlanner.cpp.

References _id_map, _nodes, node, and carla::agents::navigation::GlobalRoutePlanner::Node::vertex.

Referenced by BuildGraph().

◆ LaneChangeLink()

void carla::agents::navigation::GlobalRoutePlanner::LaneChangeLink ( )

private

Definition at line 272 of file GlobalRoutePlanner.cpp.

Referenced by GlobalRoutePlanner().

◆ Localize()

std::optional< std::pair< GlobalRoutePlanner::NodeId, GlobalRoutePlanner::NodeId > > carla::agents::navigation::GlobalRoutePlanner::Localize ( const geom::Location & loc ) const

private

Definition at line 352 of file GlobalRoutePlanner.cpp.

References _map, and _road_id_to_edge.

Referenced by LaneChangeLink(), and PathSearch().

◆ PathSearch()

std::vector< GlobalRoutePlanner::NodeId > carla::agents::navigation::GlobalRoutePlanner::PathSearch	(	const geom::Location &	origin,
		const geom::Location &	destination
	)

private

Definition at line 382 of file GlobalRoutePlanner.cpp.

References _nodes, DistanceHeuristic(), FindEdge(), carla::agents::navigation::GlobalRoutePlanner::Edge::length, and Localize().

Referenced by TraceRoute().

◆ SuccessiveLastIntersectionEdge()

std::pair< std::optional< GlobalRoutePlanner::NodeId >, const GlobalRoutePlanner::Edge * > carla::agents::navigation::GlobalRoutePlanner::SuccessiveLastIntersectionEdge	(	std::size_t	index,
		const std::vector< NodeId > &	route
	)		const

private

Definition at line 450 of file GlobalRoutePlanner.cpp.

References FindEdge(), carla::agents::navigation::GlobalRoutePlanner::Edge::intersection, carla::agents::navigation::LaneFollow, and carla::agents::navigation::GlobalRoutePlanner::Edge::type.

Referenced by TurnDecision().

◆ TraceRoute()

std::vector< GlobalRoutePlanner::RouteItem > carla::agents::navigation::GlobalRoutePlanner::TraceRoute	(	const geom::Location &	origin,
		const geom::Location &	destination
	)

Returns the list of (waypoint, RoadOption) pairs from origin to destination.

Mirrors Python trace_route.

Definition at line 610 of file GlobalRoutePlanner.cpp.

References _map, _road_id_to_edge, _sampling_resolution, carla::agents::navigation::GlobalRoutePlanner::Edge::entry_waypoint, carla::agents::navigation::GlobalRoutePlanner::Edge::exit_waypoint, FindClosestInList(), FindEdge(), carla::agents::navigation::LaneFollow, carla::agents::navigation::GlobalRoutePlanner::Edge::path, PathSearch(), TurnDecision(), carla::agents::navigation::GlobalRoutePlanner::Edge::type, and carla::agents::navigation::Void.

◆ TurnDecision()

RoadOption carla::agents::navigation::GlobalRoutePlanner::TurnDecision	(	std::size_t	index,
		const std::vector< NodeId > &	route
	)

private

Definition at line 476 of file GlobalRoutePlanner.cpp.

Referenced by TraceRoute().

Member Data Documentation

◆ _edges

std::map<std::pair<NodeId, NodeId>, Edge> carla::agents::navigation::GlobalRoutePlanner::_edges

private

Definition at line 112 of file GlobalRoutePlanner.h.

Referenced by AddEdge(), BuildGraph(), FindEdge(), and FindEdge().

◆ _id_map

std::map<XYZKey, NodeId> carla::agents::navigation::GlobalRoutePlanner::_id_map

private

Definition at line 113 of file GlobalRoutePlanner.h.

Referenced by BuildGraph(), FindLooseEnds(), GetOrCreateNode(), and LaneChangeLink().

◆ _intersection_end_node

NodeId carla::agents::navigation::GlobalRoutePlanner::_intersection_end_node = -1

private

Definition at line 121 of file GlobalRoutePlanner.h.

Referenced by TurnDecision().

◆ _map

SharedPtr<client::Map> carla::agents::navigation::GlobalRoutePlanner::_map

private

Definition at line 108 of file GlobalRoutePlanner.h.

Referenced by BuildTopology(), Localize(), and TraceRoute().

◆ _nodes

std::unordered_map<NodeId, Node> carla::agents::navigation::GlobalRoutePlanner::_nodes

private

Definition at line 111 of file GlobalRoutePlanner.h.

Referenced by AddEdge(), BuildGraph(), DistanceHeuristic(), FindLooseEnds(), GetOrCreateNode(), PathSearch(), and TurnDecision().

◆ _previous_decision

RoadOption carla::agents::navigation::GlobalRoutePlanner::_previous_decision = RoadOption::Void

private

Definition at line 122 of file GlobalRoutePlanner.h.

Referenced by TurnDecision().

◆ _road_id_to_edge

std::map<uint32_t, std::map<uint32_t, std::map<int32_t, std::pair<NodeId, NodeId> > > > carla::agents::navigation::GlobalRoutePlanner::_road_id_to_edge

private

Definition at line 117 of file GlobalRoutePlanner.h.

Referenced by BuildGraph(), FindLooseEnds(), Localize(), and TraceRoute().

◆ _sampling_resolution

double carla::agents::navigation::GlobalRoutePlanner::_sampling_resolution

private

Definition at line 109 of file GlobalRoutePlanner.h.

Referenced by BuildTopology(), FindLooseEnds(), and TraceRoute().

◆ _topology

std::vector<TopologySegment> carla::agents::navigation::GlobalRoutePlanner::_topology

private

Definition at line 110 of file GlobalRoutePlanner.h.

Referenced by BuildGraph(), BuildTopology(), FindLooseEnds(), and LaneChangeLink().

The documentation for this class was generated from the following files:

Classes

Public Types

Public Member Functions

Private Types

Private Member Functions

Static Private Member Functions

Private Attributes

Detailed Description

Member Typedef Documentation

◆ NodeId

◆ RouteItem

◆ XYZKey

Constructor & Destructor Documentation

◆ GlobalRoutePlanner()

Member Function Documentation

◆ AddEdge()

◆ BuildGraph()

◆ BuildTopology()

◆ DistanceHeuristic()

◆ FindClosestInList()

◆ FindEdge() [1/2]

◆ FindEdge() [2/2]

◆ FindLooseEnds()

◆ GetOrCreateNode()

◆ LaneChangeLink()

◆ Localize()

◆ PathSearch()

◆ SuccessiveLastIntersectionEdge()

◆ TraceRoute()

◆ TurnDecision()

Member Data Documentation

◆ _edges

◆ _id_map

◆ _intersection_end_node

◆ _map

◆ _nodes

◆ _previous_decision

◆ _road_id_to_edge

◆ _sampling_resolution

◆ _topology