DubinsPlanner object.

#include <DubinsPlanner.hpp>

Public Member Functions
	DubinsPlanner ()
	Construct a new Dubins Planner object.

void	Reset ()
	Reset object.

void	ShrinkTrafficVolume (double factor)
	Shrink traffic volume by given factor. More...

void	SetParameters (DubinsParams_t &prms)
	Set the Parameters object. More...

void	SetVehicleInitialConditions (larcfm::Vect3 &pos, larcfm::Velocity &vel)
	Set the Vehicle Initial Conditions. More...

void	SetTraffic (std::vector< larcfm::Vect3 > &tfpos, std::vector< larcfm::Velocity > &tfvel)
	Set intruders. More...

void	SetTraffic (larcfm::Plan &tf)
	Set the Traffic plan if intent information is already known. More...

void	SetBoundary (larcfm::Poly3D &boundingBox)
	Set bounding box constraint. More...

void	SetZBoundary (double zmin, double zmax)
	Set vertical limits. More...

void	SetObstacles (std::list< larcfm::Poly3D > &obstacleList)
	Input obstacles. More...

void	SetGoal (larcfm::Vect3 &goal, larcfm::Velocity finalVel)
	Set goal position and velocity. More...

bool	ComputePath (double startTime)
	Compute path. More...

void	GetPlan (larcfm::EuclideanProjection &proj, larcfm::Plan &plan)
	Get plan. More...

Static Public Member Functions
static bool	LinePlanIntersection (larcfm::Vect2 &A, larcfm::Vect2 &B, double floor, double ceiling, larcfm::Vect3 &currentPos, larcfm::Vect3 &nextPos)
	Compute intersection between line and plane. More...

Public Attributes
std::vector< node >	path
	final solution

Private Member Functions
node *	FindNearest (node &query)
	Find nearest node to a given query node. More...

void	GetPotentialFixes ()
	Compute potential fixes used for planning. More...

bool	CheckConflictCircleCircle (larcfm::Vect3 center1, larcfm::Vect3 start1, larcfm::Vect3 stop1, larcfm::Velocity startVel1, larcfm::Vect2 timeInterval1, double radius1, larcfm::Vect3 center2, larcfm::Vect3 start2, larcfm::Vect3 stop2, larcfm::Velocity startVel2, larcfm::Vect2 timeInterval2, double radius2)
	Check if two circle arcs intersect. Each arc is occupied for a certain duration. More...

bool	CheckConflictLineCircle (larcfm::Vect3 center1, larcfm::Vect3 start1, larcfm::Vect3 stop1, double turnRate, larcfm::Vect2 timeInterval1, double radius1, larcfm::Vect3 start2, larcfm::Vect3 startVel2, double t2, larcfm::Vect2 timeInterval2)
	Check intersection of line segment and arc segment. More...

std::pair< double, double >	GetLineCircleIntersection (larcfm::Vect2 start, larcfm::Vect2 direction, double t0, double R)
	Compute line circle intersection. circle centered at origin. More...

std::pair< double, double >	GetLineLineIntersection (larcfm::Vect3 start1, larcfm::Vect3 direction1, double t1, larcfm::Vect3 start2, larcfm::Vect3 direction2, double t2, double R)
	Compute Line Line Intersection. More...

bool	CheckConflictLineLine (larcfm::Vect3 start1, larcfm::Velocity startVel1, double t1, larcfm::Vect2 timeInterval1, larcfm::Vect3 start2, larcfm::Velocity startVel2, double t2, larcfm::Vect2 timeInterval2)
	Check conflict between line-line segment. Line segments have durations associated with them. More...

void	BuildTree (node *nd)
	Build tree by recursively expanding nodes. More...

bool	AstarSearch (node root, node goal)
	Perform Astar search from root to goal. More...

bool	GetNextTrkVs (node &node, double &trk, double &vs)
	Get next trk and gs to use based on the smallest cost to go child. More...

double	NodeDistance (node &A, node &B)
	Distance between two given nodes. More...

bool	CheckGoal ()
	Check if goal was reached. More...

bool	CheckProjectedFenceConflict (node qnode, node goal)
	Check if line connecting two nodes is conflicting with available fences. More...

bool	GetDubinsParams (node start, node end)
	Get parameters describing dubins curves between two nodes. More...

bool	CheckAltFeasibility (double startz, double endz, double dist, double gs1, double gs2)
	Check if altitude can be reached within the given distance and speed constraints. More...

tcpData_t	ComputeAltTcp (tcpData_t &TCPdata, double startgs, double stopgs)
	Compute alittude trajectory change points. More...

tcpData_t	ComputeSpeedTcp (tcpData_t &TCPdata, double startgs, double stopgs)
	Compute speed trajectory change points. More...

bool	CheckFenceConflict (tcpData_t trajectory)
	Check fence conflict for given dubins curve. More...

bool	CheckTrafficConflict (tcpData_t trajectory)
	Check traffic conflict for given dubins curve. More...

bool	CheckTrafficConflict (double startTime, larcfm::Vect3 center1, larcfm::Vect3 startPos, larcfm::Vect3 tcp1, larcfm::Vect3 center2, larcfm::Vect3 tcp2, larcfm::Vect3 endPos, double r, double t1, double t2, double t3, double gs, double vs)
	Check traffic conflict given a curve segment. More...

std::list< node > *	GetNodeList ()

Private Attributes
larcfm::Poly3D	boundingBox
	boundingBox defines the extent of the search space

std::vector< larcfm::Vect2 >	shrunkbbox
	contracted vertices of bounding box

std::list< larcfm::Poly3D >	obstacleList
	list of obstacles

std::vector< node >	potentialFixes
	feasible nodes

int	nodeCount
	Total node explored.

larcfm::Vect3	rootFix
	root position

larcfm::Velocity	rootVel
	velocity at root

larcfm::Vect3	goalFix
	goal position

larcfm::Velocity	goalVel
	velocity at goal

std::vector< larcfm::Vect3 >	trafficPosition
	positions of traffic

std::vector< larcfm::Velocity >	trafficVelocity
	velocities of traffic

std::list< larcfm::Plan >	trafficPlans
	intent information of traffic

DubinsParams_t	params
	parameters for dubins planner

Member Function Documentation

◆ AstarSearch()

bool DubinsPlanner::AstarSearch	(	node *	root,
		node *	goal
	)

private

Parameters

root
goal

Returns: true; false

◆ BuildTree()

void DubinsPlanner::BuildTree ( node * nd )

private

Parameters

nd

Build a directed graph by connecting fixes based on line of sight constraints

Can ignore or include reflexive transition if needed

Add fix as a child only if line of sight is available

Recursively find suitlable child fixes

◆ CheckAltFeasibility()

bool DubinsPlanner::CheckAltFeasibility	(	double	startz,
		double	endz,
		double	dist,
		double	gs1,
		double	gs2
	)

private

Parameters

startz
endz
dist
gs1
gs2

Returns: true; false

Get initial deceleration/acceleration to vc from v0

Get time to reach v1 from vc with accelration ax: (ax is deceleration if vc > v1)

◆ CheckConflictCircleCircle()

bool DubinsPlanner::CheckConflictCircleCircle	(	larcfm::Vect3	center1,
		larcfm::Vect3	start1,
		larcfm::Vect3	stop1,
		larcfm::Velocity	startVel1,
		larcfm::Vect2	timeInterval1,
		double	radius1,
		larcfm::Vect3	center2,
		larcfm::Vect3	start2,
		larcfm::Vect3	stop2,
		larcfm::Velocity	startVel2,
		larcfm::Vect2	timeInterval2,
		double	radius2
	)

private

Parameters

center1	center of arc1
start1	start point on arc1
stop1	stop point on arc1
startVel1	start velocity on arc1
timeInterval1	time interval on arc1
radius1	radius of arc1
center2	center of arc2
start2	start point on arc2
stop2	stop point on arc2
startVel2	start velocity on arc2
timeInterval2	time interval on arc2
radius2	radius of arc2

Returns: true if there is a conflict

◆ CheckConflictLineCircle()

bool DubinsPlanner::CheckConflictLineCircle	(	larcfm::Vect3	center1,
		larcfm::Vect3	start1,
		larcfm::Vect3	stop1,
		double	turnRate,
		larcfm::Vect2	timeInterval1,
		double	radius1,
		larcfm::Vect3	start2,
		larcfm::Vect3	startVel2,
		double	t2,
		larcfm::Vect2	timeInterval2
	)

private

Parameters

center1	center of arc1
start1	start point of arc1
stop1	stop point of arc1
turnRate	turn rate on arc
timeInterval1	time interval on arc1
radius1	radius of arc1
start2	start point on line
startVel2	start velocity on line
t2	start time on line
timeInterval2	time interval on line

Returns: true; false

◆ CheckConflictLineLine()

bool DubinsPlanner::CheckConflictLineLine	(	larcfm::Vect3	start1,
		larcfm::Velocity	startVel1,
		double	t1,
		larcfm::Vect2	timeInterval1,
		larcfm::Vect3	start2,
		larcfm::Velocity	startVel2,
		double	t2,
		larcfm::Vect2	timeInterval2
	)

private

Parameters

start1
startVel1
t1
timeInterval1
start2
startVel2
t2
timeInterval2

Returns: true; false

◆ CheckFenceConflict()

bool DubinsPlanner::CheckFenceConflict ( tcpData_t trajectory )

private

Parameters

trajectory

Returns: true; false

Check for fence conflict between point A and point B

For a BOT, find intersection of complete turn circle with fence vertices. Note: This is overly conservative

For all other types of TCPs, find linear intersection

◆ CheckGoal()

bool DubinsPlanner::CheckGoal ( )

private

Returns: true; false

◆ CheckProjectedFenceConflict()

bool DubinsPlanner::CheckProjectedFenceConflict	(	node *	qnode,
		node *	goal
	)

private

Parameters

qnode
goal

Returns: true; false

◆ CheckTrafficConflict() [1/2]

bool DubinsPlanner::CheckTrafficConflict	(	double	startTime,
		larcfm::Vect3	center1,
		larcfm::Vect3	startPos,
		larcfm::Vect3	tcp1,
		larcfm::Vect3	center2,
		larcfm::Vect3	tcp2,
		larcfm::Vect3	endPos,
		double	r,
		double	t1,
		double	t2,
		double	t3,
		double	gs,
		double	vs
	)

private

Parameters

startTime
center1
startPos
tcp1
center2
tcp2
endPos
r
t1
t2
t3
gs
vs

Returns: true; false

Check for conflict with the first turn segment

Check for conflict with level flight segment

Check for conflict with last turn segment

◆ CheckTrafficConflict() [2/2]

bool DubinsPlanner::CheckTrafficConflict ( tcpData_t trajectory )

private

Parameters

trajectory

Returns: true; false

Currently only handles trajectories with no overlapping TCP types Assumes traffic plans are linear

◆ ComputeAltTcp()

tcpData_t DubinsPlanner::ComputeAltTcp	(	tcpData_t &	TCPdata,
		double	startgs,
		double	stopgs
	)

private

Parameters

TCPdata
startgs
stopgs

Returns: tcpData_t

The formulation here assumes the vc < v0 and h5 > h0.
If the above assumption is not true, then hdot,az,daz,ax,dax signs must be chosen appropriately.

Get initial deceleration/acceleration to vc from v0

Expression for x,h after initial vertical acceleration/deceleration from 0 to hdot

Expression for x,h after steady climb

Time to reach v1 from vc with accelration ax: (ax is deceleration if vc > v1)

Use the BOT from original TCP

Set EOT to also be BGS

Set EGS & BVS at x1 distance from prev EOT

Set EVS at x2 distance from prev EOT

Set BVS at x3 from prev EOT

Set EVS/BGS at x4 from prev EOT

Set EGS at x5 from prev EOT

Add remaining TCPs from original data distance to BOT from x5

◆ ComputePath()

bool DubinsPlanner::ComputePath ( double startTime )

Parameters

startTime

Returns: true; false

Compute potential fix points

Build graph

search path using Astar algorithm

◆ ComputeSpeedTcp()

tcpData_t DubinsPlanner::ComputeSpeedTcp	(	tcpData_t &	TCPdata,
		double	startgs,
		double	stopgs
	)

private

Parameters

TCPdata
startgs
stopgs

Returns: tcpData_t

Get time to reach new speed based on acceleration profile

Get distance to complete acceleration to new new speed

Get segment distance before staring acceleration

Compute intermediate point for BGS (assume EGS is colocated with BOT)

◆ FindNearest()

node * DubinsPlanner::FindNearest ( node & query )

private

Parameters

query

Returns: node*

◆ GetDubinsParams()

bool DubinsPlanner::GetDubinsParams	(	node *	start,
		node *	end
	)

private

Parameters

start
end

Returns: true; false

Dubins parameter computation based on: Small Unmanned Aircraft: Theory and Practice. Randal W. Beard and Timothy W. McLain

Parameters for RSR curve

Parameters for RSL curve

Parameters for LSR curve

Parameters for LSL curve

Sort the paths based on segment lengths

Compute tcps for altitude changes

Check for fence conflicts

Check traffic conflicts on the path

◆ GetLineCircleIntersection()

std::pair< double, double > DubinsPlanner::GetLineCircleIntersection	(	larcfm::Vect2	start,
		larcfm::Vect2	direction,
		double	t0,
		double	R
	)

private

Parameters

start	start point
direction	velocity
t0	start time
R	radius of circle

Returns: std::pair<double,double>

Find intersection between a line and circle of radius r centered at (0,0) Assume a line is parameterized as follows. where start \in R2, direction \in R2, t,t0 \in R line = start + (t-t0)*direction.

◆ GetLineLineIntersection()

std::pair< double, double > DubinsPlanner::GetLineLineIntersection	(	larcfm::Vect3	start1,
		larcfm::Vect3	direction1,
		double	t1,
		larcfm::Vect3	start2,
		larcfm::Vect3	direction2,
		double	t2,
		double	R
	)

private

Parameters

start1	start point on line1
direction1	direction of line1
t1	start time on line1
start2	start point on line2
direction2	direction on line2
t2	start time on line3
R	collision radius. Consider intersection if points are R distance from each other.

Returns: std::pair<double,double>

Check if two vehicles travelling in a straight line come within D distance apart of each other. Xa = [xa,ya,za] - starting position of vehicle A Va = [vxa,vya,vza] - starting velocity of vehicle A Xb = [xb,yb,zb] - starting position of vehicle B Vb = [vxb,vyb,vzb] - starting velocity of vehicle B Xa(t) = Xa(t01) + Va x (t - t01) Xb(t) = Xb(t01) + Vb x (t - t02) Xr(t) = Xb(t) - Xa(t) Check \Forall t \in [t0,tf], ||Xr(t)|| > D Ensured by checking for the roots of quadratic equation ||Xr(t)||^2 - D^2 = 0

◆ GetNextTrkVs()

bool DubinsPlanner::GetNextTrkVs	(	node &	node,
		double &	trk,
		double &	vs
	)

private

Parameters

node
trk
vs

Returns: true; false

◆ GetPlan()

void DubinsPlanner::GetPlan	(	larcfm::EuclideanProjection &	proj,
		larcfm::Plan &	plan
	)

Parameters

proj	projection to compute gps coordinates from
plan	output

The first node in the plan is produced externally

Convert TCP center from NED frame to Geodetetic reference frame

Since the first TCP in this node is the same as the last TCP from the previous node, merge TCP data

Add MOT TCP data if making a turn > 180 degree

◆ GetPotentialFixes()

void DubinsPlanner::GetPotentialFixes ( )

private

Root and Goal nodes are always assumed feasible locations

Use POLYCarp class for detecting obstacle violations

Get positons close to each obstacle vertex as a potential fix

Expand each obstacle by 2.1 times the turn radius

Add each expanded vertex to potential fixes

add points dH above floor and root points

add points at goal altitude (after checking to see if free from obstacle conflict)

add points at current altitude

Shrink bounding box and it's vertices to potential fixes. Shrink by 2.1 x turn radius

Characteristic length used to radially sample points around root and goal nodes

Generate points radially from the root position N defines the number of paritions of [0,360]

◆ LinePlanIntersection()

bool DubinsPlanner::LinePlanIntersection	(	larcfm::Vect2 &	A,
		larcfm::Vect2 &	B,
		double	floor,
		double	ceiling,
		larcfm::Vect3 &	currentPos,
		larcfm::Vect3 &	nextPos
	)

static

Parameters

A	point A on plane
B	point B on plane
floor	plane floor
ceiling	plane ceiling
currentPos	start position of line
nextPos	end position of line

Returns: true; false

Equation of plan is given by: (p-p0).n = 0 ( '.' here is the dot product between vectors) where p0 is a point on the plane, n is the unit normal vector of the plane. Equation of line: p = l0 + d*l where l0 is a point on the line, d is a scalar, l is a vector in the line's direction. Subsitute line equation into plan to find intersection. Solve for d: d = (p0 - l0).n/(l.n) Use d in equation of line to get the point of intersection with the plane.

◆ NodeDistance()

double DubinsPlanner::NodeDistance	(	node &	A,
		node &	B
	)

private

Parameters

A
B

Returns: double

◆ SetBoundary()

void DubinsPlanner::SetBoundary ( larcfm::Poly3D & boundingBox )

Parameters

boundingBox

◆ SetGoal()

void DubinsPlanner::SetGoal	(	larcfm::Vect3 &	goal,
		larcfm::Velocity	finalVel
	)

Parameters

goal
finalVel

◆ SetObstacles()

void DubinsPlanner::SetObstacles ( std::list< larcfm::Poly3D > & obstacleList )

Parameters

obstacleList

◆ SetParameters()

void DubinsPlanner::SetParameters ( DubinsParams_t & prms )

Parameters

prms

◆ SetTraffic() [1/2]

void DubinsPlanner::SetTraffic ( larcfm::Plan & tf )

Parameters

tf

◆ SetTraffic() [2/2]

void DubinsPlanner::SetTraffic	(	std::vector< larcfm::Vect3 > &	tfpos,
		std::vector< larcfm::Velocity > &	tfvel
	)

Parameters

tfpos	list of traffic positions
tfvel	list of corresponding traffic velocities

◆ SetVehicleInitialConditions()

void DubinsPlanner::SetVehicleInitialConditions	(	larcfm::Vect3 &	pos,
		larcfm::Velocity &	vel
	)

Parameters

pos	vehicle initial position
vel	vehicle initial velocity

◆ SetZBoundary()

void DubinsPlanner::SetZBoundary	(	double	zmin,
		double	zmax
	)

Parameters

zmin
zmax

◆ ShrinkTrafficVolume()

void DubinsPlanner::ShrinkTrafficVolume ( double factor )

Parameters

factor value < 1

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

Modules/Core/TrajectoryManager/DubinsPlanner/DubinsPlanner.hpp
Modules/Core/TrajectoryManager/DubinsPlanner/DubinsPlanner.cpp
Modules/Core/TrajectoryManager/DubinsPlanner/SearchAlgorithms.cpp

Public Member Functions

Static Public Member Functions

Public Attributes

Private Member Functions

Private Attributes

Member Function Documentation

◆ AstarSearch()

◆ BuildTree()

◆ CheckAltFeasibility()

◆ CheckConflictCircleCircle()

◆ CheckConflictLineCircle()

◆ CheckConflictLineLine()

◆ CheckFenceConflict()

◆ CheckGoal()

◆ CheckProjectedFenceConflict()

◆ CheckTrafficConflict() [1/2]

◆ CheckTrafficConflict() [2/2]

◆ ComputeAltTcp()

◆ ComputePath()

◆ ComputeSpeedTcp()

◆ FindNearest()

◆ GetDubinsParams()

◆ GetLineCircleIntersection()

◆ GetLineLineIntersection()

◆ GetNextTrkVs()

◆ GetPlan()

◆ GetPotentialFixes()

◆ LinePlanIntersection()

◆ NodeDistance()

◆ SetBoundary()

◆ SetGoal()

◆ SetObstacles()

◆ SetParameters()

◆ SetTraffic() [1/2]

◆ SetTraffic() [2/2]

◆ SetVehicleInitialConditions()

◆ SetZBoundary()

◆ ShrinkTrafficVolume()